[linux-2.6-block.git] / net / rose / rose_subr.c

/*
 * This program 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.
 *
 * Copyright (C) Jonathan Naylor G4KLX (g4klx@g4klx.demon.co.uk)
 */
#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/slab.h>
#include <net/ax25.h>
#include <linux/inet.h>
#include <linux/netdevice.h>
#include <linux/skbuff.h>
#include <net/sock.h>
#include <net/tcp_states.h>
#include <asm/system.h>
#include <linux/fcntl.h>
#include <linux/mm.h>
#include <linux/interrupt.h>
#include <net/rose.h>

static int rose_create_facilities(unsigned char *buffer, struct rose_sock *rose);

/*
 *	This routine purges all of the queues of frames.
 */
void rose_clear_queues(struct sock *sk)
{
	skb_queue_purge(&sk->sk_write_queue);
	skb_queue_purge(&rose_sk(sk)->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 rose_frames_acked(struct sock *sk, unsigned short nr)
{
	struct sk_buff *skb;
	struct rose_sock *rose = rose_sk(sk);

	/*
	 * Remove all the ack-ed frames from the ack queue.
	 */
	if (rose->va != nr) {
		while (skb_peek(&rose->ack_queue) != NULL && rose->va != nr) {
			skb = skb_dequeue(&rose->ack_queue);
			kfree_skb(skb);
			rose->va = (rose->va + 1) % ROSE_MODULUS;
		}
	}
}

void rose_requeue_frames(struct sock *sk)
{
	struct sk_buff *skb, *skb_prev = NULL;

	/*
	 * Requeue all the un-ack-ed frames on the output queue to be picked
	 * up by rose_kick. This arrangement handles the possibility of an
	 * empty output queue.
	 */
	while ((skb = skb_dequeue(&rose_sk(sk)->ack_queue)) != NULL) {
		if (skb_prev == NULL)
			skb_queue_head(&sk->sk_write_queue, skb);
		else
			skb_append(skb_prev, skb, &sk->sk_write_queue);
		skb_prev = skb;
	}
}

/*
 *	Validate that the value of nr is between va and vs. Return true or
 *	false for testing.
 */
int rose_validate_nr(struct sock *sk, unsigned short nr)
{
	struct rose_sock *rose = rose_sk(sk);
	unsigned short vc = rose->va;

	while (vc != rose->vs) {
		if (nr == vc) return 1;
		vc = (vc + 1) % ROSE_MODULUS;
	}

	return nr == rose->vs;
}

/*
 *  This routine is called when the packet layer internally generates a
 *  control frame.
 */
void rose_write_internal(struct sock *sk, int frametype)
{
	struct rose_sock *rose = rose_sk(sk);
	struct sk_buff *skb;
	unsigned char  *dptr;
	unsigned char  lci1, lci2;
	char buffer[100];
	int len, faclen = 0;

	len = AX25_BPQ_HEADER_LEN + AX25_MAX_HEADER_LEN + ROSE_MIN_LEN + 1;

	switch (frametype) {
	case ROSE_CALL_REQUEST:
		len   += 1 + ROSE_ADDR_LEN + ROSE_ADDR_LEN;
		faclen = rose_create_facilities(buffer, rose);
		len   += faclen;
		break;
	case ROSE_CALL_ACCEPTED:
	case ROSE_CLEAR_REQUEST:
	case ROSE_RESET_REQUEST:
		len   += 2;
		break;
	}

	if ((skb = alloc_skb(len, GFP_ATOMIC)) == NULL)
		return;

	/*
	 *	Space for AX.25 header and PID.
	 */
	skb_reserve(skb, AX25_BPQ_HEADER_LEN + AX25_MAX_HEADER_LEN + 1);

	dptr = skb_put(skb, skb_tailroom(skb));

	lci1 = (rose->lci >> 8) & 0x0F;
	lci2 = (rose->lci >> 0) & 0xFF;

	switch (frametype) {
	case ROSE_CALL_REQUEST:
		*dptr++ = ROSE_GFI | lci1;
		*dptr++ = lci2;
		*dptr++ = frametype;
		*dptr++ = 0xAA;
		memcpy(dptr, &rose->dest_addr,  ROSE_ADDR_LEN);
		dptr   += ROSE_ADDR_LEN;
		memcpy(dptr, &rose->source_addr, ROSE_ADDR_LEN);
		dptr   += ROSE_ADDR_LEN;
		memcpy(dptr, buffer, faclen);
		dptr   += faclen;
		break;

	case ROSE_CALL_ACCEPTED:
		*dptr++ = ROSE_GFI | lci1;
		*dptr++ = lci2;
		*dptr++ = frametype;
		*dptr++ = 0x00;		/* Address length */
		*dptr++ = 0;		/* Facilities length */
		break;

	case ROSE_CLEAR_REQUEST:
		*dptr++ = ROSE_GFI | lci1;
		*dptr++ = lci2;
		*dptr++ = frametype;
		*dptr++ = rose->cause;
		*dptr++ = rose->diagnostic;
		break;

	case ROSE_RESET_REQUEST:
		*dptr++ = ROSE_GFI | lci1;
		*dptr++ = lci2;
		*dptr++ = frametype;
		*dptr++ = ROSE_DTE_ORIGINATED;
		*dptr++ = 0;
		break;

	case ROSE_RR:
	case ROSE_RNR:
		*dptr++ = ROSE_GFI | lci1;
		*dptr++ = lci2;
		*dptr   = frametype;
		*dptr++ |= (rose->vr << 5) & 0xE0;
		break;

	case ROSE_CLEAR_CONFIRMATION:
	case ROSE_RESET_CONFIRMATION:
		*dptr++ = ROSE_GFI | lci1;
		*dptr++ = lci2;
		*dptr++  = frametype;
		break;

	default:
		printk(KERN_ERR "ROSE: rose_write_internal - invalid frametype %02X\n", frametype);
		kfree_skb(skb);
		return;
	}

	rose_transmit_link(skb, rose->neighbour);
}

int rose_decode(struct sk_buff *skb, int *ns, int *nr, int *q, int *d, int *m)
{
	unsigned char *frame;

	frame = skb->data;

	*ns = *nr = *q = *d = *m = 0;

	switch (frame[2]) {
	case ROSE_CALL_REQUEST:
	case ROSE_CALL_ACCEPTED:
	case ROSE_CLEAR_REQUEST:
	case ROSE_CLEAR_CONFIRMATION:
	case ROSE_RESET_REQUEST:
	case ROSE_RESET_CONFIRMATION:
		return frame[2];
	default:
		break;
	}

	if ((frame[2] & 0x1F) == ROSE_RR  ||
	    (frame[2] & 0x1F) == ROSE_RNR) {
		*nr = (frame[2] >> 5) & 0x07;
		return frame[2] & 0x1F;
	}

	if ((frame[2] & 0x01) == ROSE_DATA) {
		*q  = (frame[0] & ROSE_Q_BIT) == ROSE_Q_BIT;
		*d  = (frame[0] & ROSE_D_BIT) == ROSE_D_BIT;
		*m  = (frame[2] & ROSE_M_BIT) == ROSE_M_BIT;
		*nr = (frame[2] >> 5) & 0x07;
		*ns = (frame[2] >> 1) & 0x07;
		return ROSE_DATA;
	}

	return ROSE_ILLEGAL;
}

static int rose_parse_national(unsigned char *p, struct rose_facilities_struct *facilities, int len)
{
	unsigned char *pt;
	unsigned char l, lg, n = 0;
	int fac_national_digis_received = 0;

	do {
		switch (*p & 0xC0) {
		case 0x00:
			p   += 2;
			n   += 2;
			len -= 2;
			break;

		case 0x40:
			if (*p == FAC_NATIONAL_RAND)
				facilities->rand = ((p[1] << 8) & 0xFF00) + ((p[2] << 0) & 0x00FF);
			p   += 3;
			n   += 3;
			len -= 3;
			break;

		case 0x80:
			p   += 4;
			n   += 4;
			len -= 4;
			break;

		case 0xC0:
			l = p[1];
			if (*p == FAC_NATIONAL_DEST_DIGI) {
				if (!fac_national_digis_received) {
					memcpy(&facilities->source_digis[0], p + 2, AX25_ADDR_LEN);
					facilities->source_ndigis = 1;
				}
			}
			else if (*p == FAC_NATIONAL_SRC_DIGI) {
				if (!fac_national_digis_received) {
					memcpy(&facilities->dest_digis[0], p + 2, AX25_ADDR_LEN);
					facilities->dest_ndigis = 1;
				}
			}
			else if (*p == FAC_NATIONAL_FAIL_CALL) {
				memcpy(&facilities->fail_call, p + 2, AX25_ADDR_LEN);
			}
			else if (*p == FAC_NATIONAL_FAIL_ADD) {
				memcpy(&facilities->fail_addr, p + 3, ROSE_ADDR_LEN);
			}
			else if (*p == FAC_NATIONAL_DIGIS) {
				fac_national_digis_received = 1;
				facilities->source_ndigis = 0;
				facilities->dest_ndigis   = 0;
				for (pt = p + 2, lg = 0 ; lg < l ; pt += AX25_ADDR_LEN, lg += AX25_ADDR_LEN) {
					if (pt[6] & AX25_HBIT)
						memcpy(&facilities->dest_digis[facilities->dest_ndigis++], pt, AX25_ADDR_LEN);
					else
						memcpy(&facilities->source_digis[facilities->source_ndigis++], pt, AX25_ADDR_LEN);
				}
			}
			p   += l + 2;
			n   += l + 2;
			len -= l + 2;
			break;
		}
	} while (*p != 0x00 && len > 0);

	return n;
}

static int rose_parse_ccitt(unsigned char *p, struct rose_facilities_struct *facilities, int len)
{
	unsigned char l, n = 0;
	char callsign[11];

	do {
		switch (*p & 0xC0) {
		case 0x00:
			p   += 2;
			n   += 2;
			len -= 2;
			break;

		case 0x40:
			p   += 3;
			n   += 3;
			len -= 3;
			break;

		case 0x80:
			p   += 4;
			n   += 4;
			len -= 4;
			break;

		case 0xC0:
			l = p[1];
			if (*p == FAC_CCITT_DEST_NSAP) {
				memcpy(&facilities->source_addr, p + 7, ROSE_ADDR_LEN);
				memcpy(callsign, p + 12,   l - 10);
				callsign[l - 10] = '\0';
				asc2ax(&facilities->source_call, callsign);
			}
			if (*p == FAC_CCITT_SRC_NSAP) {
				memcpy(&facilities->dest_addr, p + 7, ROSE_ADDR_LEN);
				memcpy(callsign, p + 12, l - 10);
				callsign[l - 10] = '\0';
				asc2ax(&facilities->dest_call, callsign);
			}
			p   += l + 2;
			n   += l + 2;
			len -= l + 2;
			break;
		}
	} while (*p != 0x00 && len > 0);

	return n;
}

int rose_parse_facilities(unsigned char *p,
	struct rose_facilities_struct *facilities)
{
	int facilities_len, len;

	facilities_len = *p++;

	if (facilities_len == 0)
		return 0;

	while (facilities_len > 0) {
		if (*p == 0x00) {
			facilities_len--;
			p++;

			switch (*p) {
			case FAC_NATIONAL:		/* National */
				len = rose_parse_national(p + 1, facilities, facilities_len - 1);
				facilities_len -= len + 1;
				p += len + 1;
				break;

			case FAC_CCITT:		/* CCITT */
				len = rose_parse_ccitt(p + 1, facilities, facilities_len - 1);
				facilities_len -= len + 1;
				p += len + 1;
				break;

			default:
				printk(KERN_DEBUG "ROSE: rose_parse_facilities - unknown facilities family %02X\n", *p);
				facilities_len--;
				p++;
				break;
			}
		} else
			break;	/* Error in facilities format */
	}

	return 1;
}

static int rose_create_facilities(unsigned char *buffer, struct rose_sock *rose)
{
	unsigned char *p = buffer + 1;
	char *callsign;
	char buf[11];
	int len, nb;

	/* National Facilities */
	if (rose->rand != 0 || rose->source_ndigis == 1 || rose->dest_ndigis == 1) {
		*p++ = 0x00;
		*p++ = FAC_NATIONAL;

		if (rose->rand != 0) {
			*p++ = FAC_NATIONAL_RAND;
			*p++ = (rose->rand >> 8) & 0xFF;
			*p++ = (rose->rand >> 0) & 0xFF;
		}

		/* Sent before older facilities */
		if ((rose->source_ndigis > 0) || (rose->dest_ndigis > 0)) {
			int maxdigi = 0;
			*p++ = FAC_NATIONAL_DIGIS;
			*p++ = AX25_ADDR_LEN * (rose->source_ndigis + rose->dest_ndigis);
			for (nb = 0 ; nb < rose->source_ndigis ; nb++) {
				if (++maxdigi >= ROSE_MAX_DIGIS)
					break;
				memcpy(p, &rose->source_digis[nb], AX25_ADDR_LEN);
				p[6] |= AX25_HBIT;
				p += AX25_ADDR_LEN;
			}
			for (nb = 0 ; nb < rose->dest_ndigis ; nb++) {
				if (++maxdigi >= ROSE_MAX_DIGIS)
					break;
				memcpy(p, &rose->dest_digis[nb], AX25_ADDR_LEN);
				p[6] &= ~AX25_HBIT;
				p += AX25_ADDR_LEN;
			}
		}

		/* For compatibility */
		if (rose->source_ndigis > 0) {
			*p++ = FAC_NATIONAL_SRC_DIGI;
			*p++ = AX25_ADDR_LEN;
			memcpy(p, &rose->source_digis[0], AX25_ADDR_LEN);
			p   += AX25_ADDR_LEN;
		}

		/* For compatibility */
		if (rose->dest_ndigis > 0) {
			*p++ = FAC_NATIONAL_DEST_DIGI;
			*p++ = AX25_ADDR_LEN;
			memcpy(p, &rose->dest_digis[0], AX25_ADDR_LEN);
			p   += AX25_ADDR_LEN;
		}
	}

	*p++ = 0x00;
	*p++ = FAC_CCITT;

	*p++ = FAC_CCITT_DEST_NSAP;

	callsign = ax2asc(buf, &rose->dest_call);

	*p++ = strlen(callsign) + 10;
	*p++ = (strlen(callsign) + 9) * 2;		/* ??? */

	*p++ = 0x47; *p++ = 0x00; *p++ = 0x11;
	*p++ = ROSE_ADDR_LEN * 2;
	memcpy(p, &rose->dest_addr, ROSE_ADDR_LEN);
	p   += ROSE_ADDR_LEN;

	memcpy(p, callsign, strlen(callsign));
	p   += strlen(callsign);

	*p++ = FAC_CCITT_SRC_NSAP;

	callsign = ax2asc(buf, &rose->source_call);

	*p++ = strlen(callsign) + 10;
	*p++ = (strlen(callsign) + 9) * 2;		/* ??? */

	*p++ = 0x47; *p++ = 0x00; *p++ = 0x11;
	*p++ = ROSE_ADDR_LEN * 2;
	memcpy(p, &rose->source_addr, ROSE_ADDR_LEN);
	p   += ROSE_ADDR_LEN;

	memcpy(p, callsign, strlen(callsign));
	p   += strlen(callsign);

	len       = p - buffer;
	buffer[0] = len - 1;

	return len;
}

void rose_disconnect(struct sock *sk, int reason, int cause, int diagnostic)
{
	struct rose_sock *rose = rose_sk(sk);

	rose_stop_timer(sk);
	rose_stop_idletimer(sk);

	rose_clear_queues(sk);

	rose->lci   = 0;
	rose->state = ROSE_STATE_0;

	if (cause != -1)
		rose->cause = cause;

	if (diagnostic != -1)
		rose->diagnostic = diagnostic;

	sk->sk_state     = TCP_CLOSE;
	sk->sk_err       = reason;
	sk->sk_shutdown |= SEND_SHUTDOWN;

	if (!sock_flag(sk, SOCK_DEAD)) {
		sk->sk_state_change(sk);
		sock_set_flag(sk, SOCK_DEAD);
	}
}
Commit	Line	Data
1da177e4 LT	1	/*
	2	* This program is free software; you can redistribute it and/or modify
	3	* it under the terms of the GNU General Public License as published by
	4	* the Free Software Foundation; either version 2 of the License, or
	5	* (at your option) any later version.
	6	*
	7	* Copyright (C) Jonathan Naylor G4KLX (g4klx@g4klx.demon.co.uk)
	8	*/
	9	#include <linux/errno.h>
	10	#include <linux/types.h>
	11	#include <linux/socket.h>
	12	#include <linux/in.h>
	13	#include <linux/kernel.h>
1da177e4 LT	14	#include <linux/timer.h>
	15	#include <linux/string.h>
	16	#include <linux/sockios.h>
	17	#include <linux/net.h>
5a0e3ad6	18	#include <linux/slab.h>
1da177e4 LT	19	#include <net/ax25.h>
	20	#include <linux/inet.h>
	21	#include <linux/netdevice.h>
	22	#include <linux/skbuff.h>
	23	#include <net/sock.h>
c752f073	24	#include <net/tcp_states.h>
1da177e4 LT	25	#include <asm/system.h>
	26	#include <linux/fcntl.h>
	27	#include <linux/mm.h>
	28	#include <linux/interrupt.h>
	29	#include <net/rose.h>
	30
	31	static int rose_create_facilities(unsigned char buffer, struct rose_sock rose);
	32
	33	/*
	34	* This routine purges all of the queues of frames.
	35	*/
	36	void rose_clear_queues(struct sock *sk)
	37	{
	38	skb_queue_purge(&sk->sk_write_queue);
	39	skb_queue_purge(&rose_sk(sk)->ack_queue);
	40	}
	41
	42	/*
	43	* This routine purges the input queue of those frames that have been
	44	* acknowledged. This replaces the boxes labelled "V(a) <- N(r)" on the
	45	* SDL diagram.
	46	*/
	47	void rose_frames_acked(struct sock *sk, unsigned short nr)
	48	{
	49	struct sk_buff *skb;
	50	struct rose_sock *rose = rose_sk(sk);
	51
	52	/*
	53	* Remove all the ack-ed frames from the ack queue.
	54	*/
	55	if (rose->va != nr) {
	56	while (skb_peek(&rose->ack_queue) != NULL && rose->va != nr) {
	57	skb = skb_dequeue(&rose->ack_queue);
	58	kfree_skb(skb);
	59	rose->va = (rose->va + 1) % ROSE_MODULUS;
	60	}
	61	}
	62	}
	63
	64	void rose_requeue_frames(struct sock *sk)
	65	{
	66	struct sk_buff skb, skb_prev = NULL;
	67
	68	/*
	69	* Requeue all the un-ack-ed frames on the output queue to be picked
	70	* up by rose_kick. This arrangement handles the possibility of an
	71	* empty output queue.
	72	*/
	73	while ((skb = skb_dequeue(&rose_sk(sk)->ack_queue)) != NULL) {
	74	if (skb_prev == NULL)
	75	skb_queue_head(&sk->sk_write_queue, skb);
	76	else
8728b834	77	skb_append(skb_prev, skb, &sk->sk_write_queue);
1da177e4 LT	78	skb_prev = skb;
	79	}
	80	}
	81
	82	/*
	83	* Validate that the value of nr is between va and vs. Return true or
	84	* false for testing.
	85	*/
	86	int rose_validate_nr(struct sock *sk, unsigned short nr)
	87	{
	88	struct rose_sock *rose = rose_sk(sk);
	89	unsigned short vc = rose->va;
	90
	91	while (vc != rose->vs) {
	92	if (nr == vc) return 1;
	93	vc = (vc + 1) % ROSE_MODULUS;
	94	}
	95
	96	return nr == rose->vs;
	97	}
	98
	99	/*
	100	* This routine is called when the packet layer internally generates a
	101	* control frame.
	102	*/
	103	void rose_write_internal(struct sock *sk, int frametype)
	104	{
	105	struct rose_sock *rose = rose_sk(sk);
	106	struct sk_buff *skb;
	107	unsigned char *dptr;
	108	unsigned char lci1, lci2;
	109	char buffer[100];
	110	int len, faclen = 0;
	111
	112	len = AX25_BPQ_HEADER_LEN + AX25_MAX_HEADER_LEN + ROSE_MIN_LEN + 1;
	113
	114	switch (frametype) {
	115	case ROSE_CALL_REQUEST:
	116	len += 1 + ROSE_ADDR_LEN + ROSE_ADDR_LEN;
	117	faclen = rose_create_facilities(buffer, rose);
	118	len += faclen;
	119	break;
	120	case ROSE_CALL_ACCEPTED:
	121	case ROSE_CLEAR_REQUEST:
	122	case ROSE_RESET_REQUEST:
	123	len += 2;
	124	break;
	125	}
	126
	127	if ((skb = alloc_skb(len, GFP_ATOMIC)) == NULL)
	128	return;
	129
	130	/*
	131	* Space for AX.25 header and PID.
	132	*/
	133	skb_reserve(skb, AX25_BPQ_HEADER_LEN + AX25_MAX_HEADER_LEN + 1);
	134
	135	dptr = skb_put(skb, skb_tailroom(skb));
	136
	137	lci1 = (rose->lci >> 8) & 0x0F;
	138	lci2 = (rose->lci >> 0) & 0xFF;
	139
	140	switch (frametype) {
	141	case ROSE_CALL_REQUEST:
142	*dptr++ = ROSE_GFI \| lci1;
143	*dptr++ = lci2;
144	*dptr++ = frametype;
145	*dptr++ = 0xAA;
146	memcpy(dptr, &rose->dest_addr, ROSE_ADDR_LEN);
147	dptr += ROSE_ADDR_LEN;
148	memcpy(dptr, &rose->source_addr, ROSE_ADDR_LEN);
149	dptr += ROSE_ADDR_LEN;
150	memcpy(dptr, buffer, faclen);
151	dptr += faclen;
152	break;
153
154	case ROSE_CALL_ACCEPTED:
155	*dptr++ = ROSE_GFI \| lci1;
156	*dptr++ = lci2;
157	*dptr++ = frametype;
158	dptr++ = 0x00; / Address length */
159	dptr++ = 0; / Facilities length */
160	break;
161
162	case ROSE_CLEAR_REQUEST:
163	*dptr++ = ROSE_GFI \| lci1;
164	*dptr++ = lci2;
165	*dptr++ = frametype;
166	*dptr++ = rose->cause;
167	*dptr++ = rose->diagnostic;
168	break;
169
170	case ROSE_RESET_REQUEST:
171	*dptr++ = ROSE_GFI \| lci1;
172	*dptr++ = lci2;
173	*dptr++ = frametype;
174	*dptr++ = ROSE_DTE_ORIGINATED;
175	*dptr++ = 0;
176	break;
177
178	case ROSE_RR:
179	case ROSE_RNR:
180	*dptr++ = ROSE_GFI \| lci1;
181	*dptr++ = lci2;
182	*dptr = frametype;
183	*dptr++ \|= (rose->vr << 5) & 0xE0;
184	break;
185
186	case ROSE_CLEAR_CONFIRMATION:
187	case ROSE_RESET_CONFIRMATION:
188	*dptr++ = ROSE_GFI \| lci1;
189	*dptr++ = lci2;
190	*dptr++ = frametype;
191	break;
192
193	default:
194	printk(KERN_ERR "ROSE: rose_write_internal - invalid frametype %02X\n", frametype);
195	kfree_skb(skb);
196	return;
197	}
198
199	rose_transmit_link(skb, rose->neighbour);
200	}
201
202	int rose_decode(struct sk_buff skb, int ns, int nr, int q, int d, int m)
203	{
204	unsigned char *frame;
205
206	frame = skb->data;
207
208	ns = nr = q = d = *m = 0;
209
210	switch (frame[2]) {
211	case ROSE_CALL_REQUEST:
212	case ROSE_CALL_ACCEPTED:
213	case ROSE_CLEAR_REQUEST:
214	case ROSE_CLEAR_CONFIRMATION:
215	case ROSE_RESET_REQUEST:
216	case ROSE_RESET_CONFIRMATION:
217	return frame[2];
218	default:
219	break;
220	}
221
222	if ((frame[2] & 0x1F) == ROSE_RR \|\|
223	(frame[2] & 0x1F) == ROSE_RNR) {
224	*nr = (frame[2] >> 5) & 0x07;
225	return frame[2] & 0x1F;
226	}
227
228	if ((frame[2] & 0x01) == ROSE_DATA) {
229	*q = (frame[0] & ROSE_Q_BIT) == ROSE_Q_BIT;
230	*d = (frame[0] & ROSE_D_BIT) == ROSE_D_BIT;
231	*m = (frame[2] & ROSE_M_BIT) == ROSE_M_BIT;
232	*nr = (frame[2] >> 5) & 0x07;
233	*ns = (frame[2] >> 1) & 0x07;
234	return ROSE_DATA;
235	}
236
237	return ROSE_ILLEGAL;
238	}
239
240	static int rose_parse_national(unsigned char p, struct rose_facilities_struct facilities, int len)
241	{
242	unsigned char *pt;
243	unsigned char l, lg, n = 0;
244	int fac_national_digis_received = 0;
245
246	do {
247	switch (*p & 0xC0) {
248	case 0x00:
249	p += 2;
250	n += 2;
251	len -= 2;
252	break;
253
254	case 0x40:
255	if (*p == FAC_NATIONAL_RAND)
256	facilities->rand = ((p[1] << 8) & 0xFF00) + ((p[2] << 0) & 0x00FF);
257	p += 3;
258	n += 3;
259	len -= 3;
260	break;
261
262	case 0x80:
263	p += 4;
264	n += 4;
265	len -= 4;
266	break;
267
268	case 0xC0:
269	l = p[1];
270	if (*p == FAC_NATIONAL_DEST_DIGI) {
271	if (!fac_national_digis_received) {
272	memcpy(&facilities->source_digis[0], p + 2, AX25_ADDR_LEN);
273	facilities->source_ndigis = 1;
274	}
275	}
276	else if (*p == FAC_NATIONAL_SRC_DIGI) {
277	if (!fac_national_digis_received) {
278	memcpy(&facilities->dest_digis[0], p + 2, AX25_ADDR_LEN);
279	facilities->dest_ndigis = 1;
280	}
281	}
282	else if (*p == FAC_NATIONAL_FAIL_CALL) {
283	memcpy(&facilities->fail_call, p + 2, AX25_ADDR_LEN);
284	}
285	else if (*p == FAC_NATIONAL_FAIL_ADD) {
286	memcpy(&facilities->fail_addr, p + 3, ROSE_ADDR_LEN);
287	}
288	else if (*p == FAC_NATIONAL_DIGIS) {
289	fac_national_digis_received = 1;
290	facilities->source_ndigis = 0;
291	facilities->dest_ndigis = 0;
292	for (pt = p + 2, lg = 0 ; lg < l ; pt += AX25_ADDR_LEN, lg += AX25_ADDR_LEN) {
293	if (pt[6] & AX25_HBIT)
294	memcpy(&facilities->dest_digis[facilities->dest_ndigis++], pt, AX25_ADDR_LEN);
295	else
296	memcpy(&facilities->source_digis[facilities->source_ndigis++], pt, AX25_ADDR_LEN);
297	}
298	}
299	p += l + 2;
300	n += l + 2;
301	len -= l + 2;
302	break;
303	}
304	} while (*p != 0x00 && len > 0);
305
306	return n;
307	}
308
309	static int rose_parse_ccitt(unsigned char p, struct rose_facilities_struct facilities, int len)
310	{
311	unsigned char l, n = 0;
312	char callsign[11];
313
314	do {
315	switch (*p & 0xC0) {
316	case 0x00:
317	p += 2;
318	n += 2;
319	len -= 2;
320	break;
321
322	case 0x40:
323	p += 3;
324	n += 3;
325	len -= 3;
326	break;
327
328	case 0x80:
329	p += 4;
330	n += 4;
331	len -= 4;
332	break;
333
334	case 0xC0:
335	l = p[1];
336	if (*p == FAC_CCITT_DEST_NSAP) {
337	memcpy(&facilities->source_addr, p + 7, ROSE_ADDR_LEN);
338	memcpy(callsign, p + 12, l - 10);
339	callsign[l - 10] = '\0';
baed16a7	340	asc2ax(&facilities->source_call, callsign);
1da177e4 LT	341	}
	342	if (*p == FAC_CCITT_SRC_NSAP) {
	343	memcpy(&facilities->dest_addr, p + 7, ROSE_ADDR_LEN);
	344	memcpy(callsign, p + 12, l - 10);
	345	callsign[l - 10] = '\0';
baed16a7	346	asc2ax(&facilities->dest_call, callsign);
1da177e4 LT	347	}
	348	p += l + 2;
	349	n += l + 2;
	350	len -= l + 2;
	351	break;
	352	}
	353	} while (*p != 0x00 && len > 0);
	354
	355	return n;
	356	}
	357
	358	int rose_parse_facilities(unsigned char *p,
	359	struct rose_facilities_struct *facilities)
	360	{
	361	int facilities_len, len;
	362
	363	facilities_len = *p++;
	364
	365	if (facilities_len == 0)
	366	return 0;
	367
	368	while (facilities_len > 0) {
	369	if (*p == 0x00) {
	370	facilities_len--;
	371	p++;
	372
	373	switch (*p) {
	374	case FAC_NATIONAL: /* National */
	375	len = rose_parse_national(p + 1, facilities, facilities_len - 1);
	376	facilities_len -= len + 1;
	377	p += len + 1;
	378	break;
	379
	380	case FAC_CCITT: /* CCITT */
	381	len = rose_parse_ccitt(p + 1, facilities, facilities_len - 1);
	382	facilities_len -= len + 1;
	383	p += len + 1;
	384	break;
	385
	386	default:
	387	printk(KERN_DEBUG "ROSE: rose_parse_facilities - unknown facilities family %02X\n", *p);
	388	facilities_len--;
	389	p++;
	390	break;
	391	}
	392	} else
	393	break; /* Error in facilities format */
	394	}
	395
	396	return 1;
	397	}
	398
	399	static int rose_create_facilities(unsigned char buffer, struct rose_sock rose)
	400	{
	401	unsigned char *p = buffer + 1;
	402	char *callsign;
f75268cd	403	char buf[11];
1da177e4 LT	404	int len, nb;
	405
	406	/* National Facilities */
	407	if (rose->rand != 0 \|\| rose->source_ndigis == 1 \|\| rose->dest_ndigis == 1) {
	408	*p++ = 0x00;
	409	*p++ = FAC_NATIONAL;
	410
	411	if (rose->rand != 0) {
	412	*p++ = FAC_NATIONAL_RAND;
	413	*p++ = (rose->rand >> 8) & 0xFF;
	414	*p++ = (rose->rand >> 0) & 0xFF;
	415	}
	416
	417	/* Sent before older facilities */
	418	if ((rose->source_ndigis > 0) \|\| (rose->dest_ndigis > 0)) {
	419	int maxdigi = 0;
	420	*p++ = FAC_NATIONAL_DIGIS;
	421	p++ = AX25_ADDR_LEN (rose->source_ndigis + rose->dest_ndigis);
	422	for (nb = 0 ; nb < rose->source_ndigis ; nb++) {
	423	if (++maxdigi >= ROSE_MAX_DIGIS)
	424	break;
	425	memcpy(p, &rose->source_digis[nb], AX25_ADDR_LEN);
	426	p[6] \|= AX25_HBIT;
	427	p += AX25_ADDR_LEN;
	428	}
	429	for (nb = 0 ; nb < rose->dest_ndigis ; nb++) {
	430	if (++maxdigi >= ROSE_MAX_DIGIS)
	431	break;
	432	memcpy(p, &rose->dest_digis[nb], AX25_ADDR_LEN);
	433	p[6] &= ~AX25_HBIT;
	434	p += AX25_ADDR_LEN;
	435	}
	436	}
	437
	438	/* For compatibility */
	439	if (rose->source_ndigis > 0) {
	440	*p++ = FAC_NATIONAL_SRC_DIGI;
	441	*p++ = AX25_ADDR_LEN;
	442	memcpy(p, &rose->source_digis[0], AX25_ADDR_LEN);
	443	p += AX25_ADDR_LEN;
	444	}
	445
	446	/* For compatibility */
	447	if (rose->dest_ndigis > 0) {
	448	*p++ = FAC_NATIONAL_DEST_DIGI;
	449	*p++ = AX25_ADDR_LEN;
	450	memcpy(p, &rose->dest_digis[0], AX25_ADDR_LEN);
	451	p += AX25_ADDR_LEN;
	452	}
	453	}
	454
	455	*p++ = 0x00;
	456	*p++ = FAC_CCITT;
	457
	458	*p++ = FAC_CCITT_DEST_NSAP;
	459
f75268cd	460	callsign = ax2asc(buf, &rose->dest_call);
1da177e4 LT	461
	462	*p++ = strlen(callsign) + 10;
	463	p++ = (strlen(callsign) + 9) 2; /* ??? */
	464
	465	p++ = 0x47; p++ = 0x00; *p++ = 0x11;
	466	p++ = ROSE_ADDR_LEN 2;
	467	memcpy(p, &rose->dest_addr, ROSE_ADDR_LEN);
	468	p += ROSE_ADDR_LEN;
	469
	470	memcpy(p, callsign, strlen(callsign));
	471	p += strlen(callsign);
	472
	473	*p++ = FAC_CCITT_SRC_NSAP;
	474
f75268cd	475	callsign = ax2asc(buf, &rose->source_call);
1da177e4 LT	476
	477	*p++ = strlen(callsign) + 10;
	478	p++ = (strlen(callsign) + 9) 2; /* ??? */
	479
	480	p++ = 0x47; p++ = 0x00; *p++ = 0x11;
	481	p++ = ROSE_ADDR_LEN 2;
	482	memcpy(p, &rose->source_addr, ROSE_ADDR_LEN);
	483	p += ROSE_ADDR_LEN;
	484
	485	memcpy(p, callsign, strlen(callsign));
	486	p += strlen(callsign);
	487
	488	len = p - buffer;
	489	buffer[0] = len - 1;
	490
	491	return len;
	492	}
	493
	494	void rose_disconnect(struct sock *sk, int reason, int cause, int diagnostic)
	495	{
	496	struct rose_sock *rose = rose_sk(sk);
	497
	498	rose_stop_timer(sk);
	499	rose_stop_idletimer(sk);
	500
	501	rose_clear_queues(sk);
	502
	503	rose->lci = 0;
	504	rose->state = ROSE_STATE_0;
	505
	506	if (cause != -1)
	507	rose->cause = cause;
	508
	509	if (diagnostic != -1)
	510	rose->diagnostic = diagnostic;
	511
	512	sk->sk_state = TCP_CLOSE;
	513	sk->sk_err = reason;
	514	sk->sk_shutdown \|= SEND_SHUTDOWN;
	515
	516	if (!sock_flag(sk, SOCK_DEAD)) {
	517	sk->sk_state_change(sk);
	518	sock_set_flag(sk, SOCK_DEAD);
	519	}
	520	}