sdl_udp.c

No7信令,我需要交换类似的代码, 请店长审核,谢谢了,急着交换,谢谢

	*lenp = count;
#if _DEBUG
	{
		int i;
		char *c;
		printk("PPA =");
		for ( i=0, c=(char *)*ppap; i<count; i++, c++ )
			printk(" %02x", *c);
		printk("\n");
	}
#endif
	return(0);
}
static int
sdl_udp_attach(lmi_t *lmi, void *ppa, int len)
{
	sdl_udpdev_t *udp = (sdl_udpdev_t *)lmi;
	size_t count = 2*sizeof(struct sockaddr_in);
	if ( len < count )
		return EINVAL | LMI_BADPPA;
	bcopy(ppa, &udp->loc_addr, count);
#if _DEBUG
	{
		int i;
		char *c;
		printk("PPA =");
		for ( i=0, c=(char *)ppa; i<count; i++, c++ )
			printk(" %02x", *c);
		printk("\n");
	}
#endif
	return(0);
}
static int
sdl_udp_detach(lmi_t *lmi)
{
	sdl_udpdev_t *udp = (sdl_udpdev_t *)lmi;
	bzero(&udp->loc_addr, 2*sizeof(struct sockaddr_in));
	return(0);
}
static void sdl_udp_data_ready(struct sock *, int);
#ifndef abs
#define abs(x) ((x)<0 ? -(x):(x))
#endif
static int
sdl_udp_enable(lmi_t *lmi)
{
	int err;
	struct socket *sock = NULL;
	sdl_udpdev_t *udp = (sdl_udpdev_t *)lmi;
	struct sockaddr *sa = (struct sockaddr *)&udp->loc_addr;
	err = sock_create(PF_INET, SOCK_DGRAM, 0, &sock);
	if ( err < 0 )
		return (abs(err)|LMI_INITFAILED);
	sock->sk->reuse = 1;    /* FIXME: check this */
	sock->sk->allocation = GFP_ATOMIC;
	err = sock->ops->bind(sock, sa, sizeof(*sa));
	if ( err < 0 )
	{
		sock_release(sock);
		return (abs(err)|LMI_BADPPA);
	}
	sock->sk->protinfo.destruct_hook = udp; /* might be bad idea */
	sock->sk->data_ready = sdl_udp_data_ready;
	udp->udpsock = sock;
	return (0);
}
static int
sdl_udp_disable(lmi_t *lmi)
{
	sdl_udpdev_t *udp = (sdl_udpdev_t *)lmi;
	struct socket *sock;
	if ( udp )
	{
		udp->dev.iface.ifflags &= ~DEV_IF_TX_RUNNING;
		if ( (sock = udp->udpsock) )
		{
			sock->sk->protinfo.destruct_hook = NULL;
			sock_release(sock);
			udp->udpsock = NULL;
			bufq_purge(&udp->txq);
			del_timer(&udp->wakeup);
		}
		udp->dev.iface.ifflags &= ~DEV_IF_RX_RUNNING;
	}
	return(0);
}
static struct lmi_ops sdl_udp_lmi_ops =
{
	{
		sdl_udp_devatt,		/* dev.attach	*/
		sdl_udp_open,		/* dev.open	*/
		sdl_udp_close		/* dev.close	*/
	},
	{
		sdl_udp_infor,		/* lmi.info	*/
		sdl_udp_attach,		/* lmi.attach	*/
		sdl_udp_detach,		/* lmi.detach	*/
		sdl_udp_enable,		/* lmi.enable	*/
		sdl_udp_disable,	/* lmi.disable	*/
		sdl_udp_ioctl		/* lmi.ioctl	*/
	}
};

/*
 *  =========================================================================
 *
 *  SERVICE ROUTINES
 *
 *  =========================================================================
 */
/*
 *  This is equivalent of RxISR.
 */
static void
sdl_udp_data_ready(struct sock *sk, int bytes)
{
	int err = 0;
	struct sk_buff *skb;
	struct dev *dev = sk->protinfo.destruct_hook;
	int rx_on = ( dev && (dev->iface.ifflags & DEV_IF_RX_RUNNING) );
	(void)bytes;
	while ( (skb = skb_recv_datagram(sk, 0, 1, &err)) )
	{
		if ( rx_on )
		{
			mblk_t *mp;
			caddr_t data = skb->h.raw + sizeof(struct udphdr);
			size_t  len  = skb->len   - sizeof(struct udphdr);
			dev->module->stats.rx_sus++;
			dev->module->stats.rx_bytes += len;
			skb->sk = sk;   /* just to be sure */
//			if ( (mp = sdl_udp_esballoc(skb, data, len)) )
			{
				if ( (mp = allocb(len, BPRI_MED)) )
				{
					bcopy(data, mp->b_wptr, len);
					mp->b_wptr += len;
					mp->b_datap->db_type = M_DATA;
					dev->ucalls->daedr_recvd_frame(dev, mp);
				}
				else
				{
					dev->module->stats.rx_overruns++;
					dev->module->stats.rx_buffer_overflows++;
					dev->ucalls->daedr_error_frame(dev);
				}
			}
		}
		skb_free_datagram(sk, skb);
	}
//	FIXME: ignore errors for now...
//	if ( err && rx_on && abs(err) != EAGAIN )
//		/* FIXME: analyze this error */
//		dev->ucalls->daedr_error_frame(dev);
	return;
}
/*
 *  This is equivalent of TxISR.
 */
static void
sdl_udp_send(struct dev *dev)
{
	int n, size, tdiff;
	mblk_t *mp, *db;
	sdl_udpdev_t *udp = (sdl_udpdev_t *)dev;
	if ( !(dev->iface.ifflags & DEV_IF_TX_RUNNING) )
		return;
	while ( (mp = bufq_head(&udp->txq)) )
	{
		if ( (tdiff = jiffies - udp->timestamp) < 0 )
		{
			ptrace(("throttling!\n"));
			mod_timer(&udp->wakeup, jiffies+1); /* throttle back */
			return;
		}
		if ( tdiff > 0 )
		{
			udp->bytecount = 0;
			udp->timestamp = jiffies;
		}
		for ( size=0, n=0, db=mp; db; db=db->b_cont )
			if ( db->b_datap->db_type == M_DATA && db->b_wptr > db->b_rptr )
			{
				size += db->b_wptr - db->b_rptr;
				n++;
			}
//		ptrace(("number M_DATA blocks = %d\n",n));
		if ( n )
		{
			int i, err=0;
			struct msghdr udpmsg;
			struct iovec iov[n];
			for ( i=0, db=mp; db; db=db->b_cont )
			{
				if ( db->b_datap->db_type == M_DATA && db->b_wptr > db->b_rptr )
				{
					iov[i].iov_base = db->b_rptr;
					iov[i].iov_len  = db->b_wptr - db->b_rptr;
					i++;
				}
			}
			udpmsg.msg_name = (void *)&udp->rem_addr;
			udpmsg.msg_namelen = sizeof(udp->rem_addr);
			udpmsg.msg_iov = iov;
			udpmsg.msg_iovlen = n;
			udpmsg.msg_control = NULL;
			udpmsg.msg_controllen = 0;
			udpmsg.msg_flags = 0;
			{
				mm_segment_t fs = get_fs();
				set_fs(KERNEL_DS);
				err = sock_sendmsg(udp->udpsock, &udpmsg, size);
				set_fs(fs);
			}
			if ( err < 0 )
			{
				ptrace(("Error = %d\n",err));
				switch ( -err )
				{
					case ENOMEM:		/* ran out of memory */
					case ENOBUFS:		/* ran out of memory */
					case EAGAIN:		/* no space on non-blocking socket */
					case ERESTARTSYS:	/* signal */
						mod_timer(&udp->wakeup, jiffies+3);
						return;		/* temporary, try again later */
					default:
					case EINVAL:		/* bad address */
					case EACCES:		/* tried to send on broadcast address from nonbroadcast socket */
					case ENODEV:		/* bad index on iface directed output */
					case EFAULT:		/* bad user space address */
					case EPERM:		/* blocked by firewall */
					case EMSGSIZE:		/* message bigger than MTU */
					case EPIPE:		/* socket closed */
					case ENOTCONN:		/* sk->state != TCP_ESTABLISHED connected socket */
					case ECONNREFUSED:	/* no receiver */
					case ENETDOWN:		/* couldn't route to destination */
					case ENETUNREACH:	/* cant route to dest */
					case EHOSTUNREACH:	/* no routing entry to dest */
						dev->module->stats.tx_sus_in_error+=bufq_length(&udp->txq);
						bufq_purge(&udp->txq);
						return;		/* bad error, link unusable */
				}
			}
			dev->module->stats.tx_bytes += size;
			dev->module->stats.tx_sus++;
			udp->bytecount += size;
			while ( udp->bytecount >= udp->tickbytes )
			{
				udp->bytecount -= udp->tickbytes;
				udp->timestamp++;
			}
		}
		if ( dev->iface.ifclock == DEV_CLOCK_TICK )
		{
			if ( bufq_length(&udp->txq) == 1 )
			{
				int lssu = ((dev->module->option.popt&SS7_POPT_XSN)?6:3)+1;
				int lss2 = lssu+1;
				/* repeat FISUs or LSSUs other than SIB until told otherwise */
				if ( size < lss2+1
						&& ( size != lssu || mp->b_rptr[lssu-1] != 0x5 )
						&& ( size != lss2 || mp->b_rptr[lss2-1] != 0x5 )
						)
				{
					mod_timer(&udp->wakeup, jiffies+3);
					return;
				}
			}
		}
		bufq_freehead(&udp->txq);
	}
	dev->ucalls->daedt_tx_request(dev);
}
static void
sdl_udp_xmit(struct dev *dev, mblk_t *mp)
{
	sdl_udpdev_t *udp = (sdl_udpdev_t *)dev;
	del_timer(&udp->wakeup);
	if ( dev->iface.ifflags & DEV_IF_TX_RUNNING )
	{
		bufq_queue(&udp->txq, mp);
		sdl_udp_send(dev);
	}
	else
	{
		assert(mp->b_datap->db_ref);
		freemsg(mp);
	}
}
static void
sdl_udp_tx_start(struct dev *dev)
{
	dev->iface.ifflags |= DEV_IF_TX_RUNNING;
	return;
}
static void
sdl_udp_rx_start(struct dev *dev)
{
	dev->iface.ifflags |= DEV_IF_RX_RUNNING;
	return;
}
static dev_dcalls_t sdl_udp_dcalls = 
{
	sdl_udp_xmit,		/* daedt_xmit	*/
	sdl_udp_tx_start,	/* daedt_start	*/
	sdl_udp_rx_start	/* daedr_start	*/
};

/*
 *  =======================================================================
 *
 *  LiS Module Initialization   (For registered driver.)
 *
 *  =======================================================================
 */
static int sdl_udp_major = SDL_UDP_CMAJOR;
static int sdl_udp_initialized = 0;
void sdl_udp_init(void)
{
	if ( sdl_udp_initialized > 0 )
		return;
	cmn_err(CE_NOTE, SDL_BANNER);   /* console splash */
//	ptrace(("registering sdl_udp: cmajor %d, nminors %d\n", SDL_UDP_CMAJOR, SDL_UDP_NMINOR));
	sdl_udp_initialized
		= sdl_register_driver(SDL_UDP_CMAJOR, SDL_UDP_NMINOR, "sdl_udp",
				&sdl_udp_lmi_ops, &sdl_udp_dcalls);
//	ptrace(("return (device cmajor) = %d\n", sdl_udp_initialized));
	if ( sdl_udp_initialized > 0 )
		sdl_udp_major = sdl_udp_initialized;
	if ( sdl_udp_initialized == 0 )
	{
		sdl_udp_initialized = sdl_udp_major;
	}
}
void sdl_udp_terminate(void)
{
	if ( sdl_udp_initialized <= 0 )
		return;
//	ptrace(("unregistering sdl_udp cmajor %d\n", sdl_udp_major));
	sdl_udp_initialized = sdl_unregister_driver(sdl_udp_major);
//	ptrace(("return = %d\n", sdl_udp_initialized));
}

/*
 *  =======================================================================
 *
 *  Kernel Module Initialization   (For unregistered driver.)
 *
 *  =======================================================================
 */
#ifdef MODULE
int init_module(void)
{
	(void)sdl_debug;
	sdl_udp_init();
	if ( sdl_udp_initialized < 0 ) return sdl_udp_initialized;
	return(0);
}
void cleanup_module(void)
{
	sdl_udp_terminate();
}
#endif