n_r3964.c

linux 内核源代码

		remove_client_block(pInfo, pClient);
		return block->length;
	}

	return -EINVAL;
}

static void add_msg(struct r3964_client_info *pClient, int msg_id, int arg,
		int error_code, struct r3964_block_header *pBlock)
{
	struct r3964_message *pMsg;
	unsigned long flags;

	if (pClient->msg_count < R3964_MAX_MSG_COUNT - 1) {
queue_the_message:

		pMsg = kmalloc(sizeof(struct r3964_message),
				error_code ? GFP_ATOMIC : GFP_KERNEL);
		TRACE_M("add_msg - kmalloc %p", pMsg);
		if (pMsg == NULL) {
			return;
		}

		spin_lock_irqsave(&pClient->lock, flags);

		pMsg->msg_id = msg_id;
		pMsg->arg = arg;
		pMsg->error_code = error_code;
		pMsg->block = pBlock;
		pMsg->next = NULL;

		if (pClient->last_msg == NULL) {
			pClient->first_msg = pClient->last_msg = pMsg;
		} else {
			pClient->last_msg->next = pMsg;
			pClient->last_msg = pMsg;
		}

		pClient->msg_count++;

		if (pBlock != NULL) {
			pBlock->locks++;
		}
		spin_unlock_irqrestore(&pClient->lock, flags);
	} else {
		if ((pClient->last_msg->msg_id == R3964_MSG_ACK)
		    && (pClient->last_msg->error_code == R3964_OVERFLOW)) {
			pClient->last_msg->arg++;
			TRACE_PE("add_msg - inc prev OVERFLOW-msg");
		} else {
			msg_id = R3964_MSG_ACK;
			arg = 0;
			error_code = R3964_OVERFLOW;
			pBlock = NULL;
			TRACE_PE("add_msg - queue OVERFLOW-msg");
			goto queue_the_message;
		}
	}
	/* Send SIGIO signal to client process: */
	if (pClient->sig_flags & R3964_USE_SIGIO) {
		kill_pid(pClient->pid, SIGIO, 1);
	}
}

static struct r3964_message *remove_msg(struct r3964_info *pInfo,
					struct r3964_client_info *pClient)
{
	struct r3964_message *pMsg = NULL;
	unsigned long flags;

	if (pClient->first_msg) {
		spin_lock_irqsave(&pClient->lock, flags);

		pMsg = pClient->first_msg;
		pClient->first_msg = pMsg->next;
		if (pClient->first_msg == NULL) {
			pClient->last_msg = NULL;
		}

		pClient->msg_count--;
		if (pMsg->block) {
			remove_client_block(pInfo, pClient);
			pClient->next_block_to_read = pMsg->block;
		}
		spin_unlock_irqrestore(&pClient->lock, flags);
	}
	return pMsg;
}

static void remove_client_block(struct r3964_info *pInfo,
				struct r3964_client_info *pClient)
{
	struct r3964_block_header *block;

	TRACE_PS("remove_client_block PID %d", pid_nr(pClient->pid));

	block = pClient->next_block_to_read;
	if (block) {
		block->locks--;
		if (block->locks == 0) {
			remove_from_rx_queue(pInfo, block);
		}
	}
	pClient->next_block_to_read = NULL;
}

/*************************************************************
 * Line discipline routines
 *************************************************************/

static int r3964_open(struct tty_struct *tty)
{
	struct r3964_info *pInfo;

	TRACE_L("open");
	TRACE_L("tty=%p, PID=%d, disc_data=%p",
		tty, current->pid, tty->disc_data);

	pInfo = kmalloc(sizeof(struct r3964_info), GFP_KERNEL);
	TRACE_M("r3964_open - info kmalloc %p", pInfo);

	if (!pInfo) {
		printk(KERN_ERR "r3964: failed to alloc info structure\n");
		return -ENOMEM;
	}

	pInfo->rx_buf = kmalloc(RX_BUF_SIZE, GFP_KERNEL);
	TRACE_M("r3964_open - rx_buf kmalloc %p", pInfo->rx_buf);

	if (!pInfo->rx_buf) {
		printk(KERN_ERR "r3964: failed to alloc receive buffer\n");
		kfree(pInfo);
		TRACE_M("r3964_open - info kfree %p", pInfo);
		return -ENOMEM;
	}

	pInfo->tx_buf = kmalloc(TX_BUF_SIZE, GFP_KERNEL);
	TRACE_M("r3964_open - tx_buf kmalloc %p", pInfo->tx_buf);

	if (!pInfo->tx_buf) {
		printk(KERN_ERR "r3964: failed to alloc transmit buffer\n");
		kfree(pInfo->rx_buf);
		TRACE_M("r3964_open - rx_buf kfree %p", pInfo->rx_buf);
		kfree(pInfo);
		TRACE_M("r3964_open - info kfree %p", pInfo);
		return -ENOMEM;
	}

	spin_lock_init(&pInfo->lock);
	pInfo->tty = tty;
	init_waitqueue_head(&pInfo->read_wait);
	pInfo->priority = R3964_MASTER;
	pInfo->rx_first = pInfo->rx_last = NULL;
	pInfo->tx_first = pInfo->tx_last = NULL;
	pInfo->rx_position = 0;
	pInfo->tx_position = 0;
	pInfo->last_rx = 0;
	pInfo->blocks_in_rx_queue = 0;
	pInfo->firstClient = NULL;
	pInfo->state = R3964_IDLE;
	pInfo->flags = R3964_DEBUG;
	pInfo->nRetry = 0;

	tty->disc_data = pInfo;
	tty->receive_room = 65536;

	setup_timer(&pInfo->tmr, on_timeout, (unsigned long)pInfo);

	return 0;
}

static void r3964_close(struct tty_struct *tty)
{
	struct r3964_info *pInfo = (struct r3964_info *)tty->disc_data;
	struct r3964_client_info *pClient, *pNext;
	struct r3964_message *pMsg;
	struct r3964_block_header *pHeader, *pNextHeader;
	unsigned long flags;

	TRACE_L("close");

	/*
	 * Make sure that our task queue isn't activated.  If it
	 * is, take it out of the linked list.
	 */
	del_timer_sync(&pInfo->tmr);

	/* Remove client-structs and message queues: */
	pClient = pInfo->firstClient;
	while (pClient) {
		pNext = pClient->next;
		while (pClient->msg_count) {
			pMsg = remove_msg(pInfo, pClient);
			if (pMsg) {
				kfree(pMsg);
				TRACE_M("r3964_close - msg kfree %p", pMsg);
			}
		}
		put_pid(pClient->pid);
		kfree(pClient);
		TRACE_M("r3964_close - client kfree %p", pClient);
		pClient = pNext;
	}
	/* Remove jobs from tx_queue: */
	spin_lock_irqsave(&pInfo->lock, flags);
	pHeader = pInfo->tx_first;
	pInfo->tx_first = pInfo->tx_last = NULL;
	spin_unlock_irqrestore(&pInfo->lock, flags);

	while (pHeader) {
		pNextHeader = pHeader->next;
		kfree(pHeader);
		pHeader = pNextHeader;
	}

	/* Free buffers: */
	wake_up_interruptible(&pInfo->read_wait);
	kfree(pInfo->rx_buf);
	TRACE_M("r3964_close - rx_buf kfree %p", pInfo->rx_buf);
	kfree(pInfo->tx_buf);
	TRACE_M("r3964_close - tx_buf kfree %p", pInfo->tx_buf);
	kfree(pInfo);
	TRACE_M("r3964_close - info kfree %p", pInfo);
}

static ssize_t r3964_read(struct tty_struct *tty, struct file *file,
			  unsigned char __user * buf, size_t nr)
{
	struct r3964_info *pInfo = (struct r3964_info *)tty->disc_data;
	struct r3964_client_info *pClient;
	struct r3964_message *pMsg;
	struct r3964_client_message theMsg;
	int count;

	TRACE_L("read()");

	pClient = findClient(pInfo, task_pid(current));
	if (pClient) {
		pMsg = remove_msg(pInfo, pClient);
		if (pMsg == NULL) {
			/* no messages available. */
			if (file->f_flags & O_NONBLOCK) {
				return -EAGAIN;
			}
			/* block until there is a message: */
			wait_event_interruptible(pInfo->read_wait,
					(pMsg = remove_msg(pInfo, pClient)));
		}

		/* If we still haven't got a message, we must have been signalled */

		if (!pMsg)
			return -EINTR;

		/* deliver msg to client process: */
		theMsg.msg_id = pMsg->msg_id;
		theMsg.arg = pMsg->arg;
		theMsg.error_code = pMsg->error_code;
		count = sizeof(struct r3964_client_message);

		kfree(pMsg);
		TRACE_M("r3964_read - msg kfree %p", pMsg);

		if (copy_to_user(buf, &theMsg, count))
			return -EFAULT;

		TRACE_PS("read - return %d", count);
		return count;
	}
	return -EPERM;
}

static ssize_t r3964_write(struct tty_struct *tty, struct file *file,
			   const unsigned char *data, size_t count)
{
	struct r3964_info *pInfo = (struct r3964_info *)tty->disc_data;
	struct r3964_block_header *pHeader;
	struct r3964_client_info *pClient;
	unsigned char *new_data;

	TRACE_L("write request, %d characters", count);
/* 
 * Verify the pointers 
 */

	if (!pInfo)
		return -EIO;

/*
 * Ensure that the caller does not wish to send too much.
 */
	if (count > R3964_MTU) {
		if (pInfo->flags & R3964_DEBUG) {
			TRACE_L(KERN_WARNING "r3964_write: truncating user "
				"packet from %u to mtu %d", count, R3964_MTU);
		}
		count = R3964_MTU;
	}
/*
 * Allocate a buffer for the data and copy it from the buffer with header prepended
 */
	new_data = kmalloc(count + sizeof(struct r3964_block_header),
			GFP_KERNEL);
	TRACE_M("r3964_write - kmalloc %p", new_data);
	if (new_data == NULL) {
		if (pInfo->flags & R3964_DEBUG) {
			printk(KERN_ERR "r3964_write: no memory\n");
		}
		return -ENOSPC;
	}

	pHeader = (struct r3964_block_header *)new_data;
	pHeader->data = new_data + sizeof(struct r3964_block_header);
	pHeader->length = count;
	pHeader->locks = 0;
	pHeader->owner = NULL;

	pClient = findClient(pInfo, task_pid(current));
	if (pClient) {
		pHeader->owner = pClient;
	}

	memcpy(pHeader->data, data, count);	/* We already verified this */

	if (pInfo->flags & R3964_DEBUG) {
		dump_block(pHeader->data, count);
	}

/*
 * Add buffer to transmit-queue:
 */
	add_tx_queue(pInfo, pHeader);
	trigger_transmit(pInfo);

	return 0;
}

static int r3964_ioctl(struct tty_struct *tty, struct file *file,
		unsigned int cmd, unsigned long arg)
{
	struct r3964_info *pInfo = (struct r3964_info *)tty->disc_data;
	if (pInfo == NULL)
		return -EINVAL;
	switch (cmd) {
	case R3964_ENABLE_SIGNALS:
		return enable_signals(pInfo, task_pid(current), arg);
	case R3964_SETPRIORITY:
		if (arg < R3964_MASTER || arg > R3964_SLAVE)
			return -EINVAL;
		pInfo->priority = arg & 0xff;
		return 0;
	case R3964_USE_BCC:
		if (arg)
			pInfo->flags |= R3964_BCC;
		else
			pInfo->flags &= ~R3964_BCC;
		return 0;
	case R3964_READ_TELEGRAM:
		return read_telegram(pInfo, task_pid(current),
				(unsigned char __user *)arg);
	default:
		return -ENOIOCTLCMD;
	}
}

static void r3964_set_termios(struct tty_struct *tty, struct ktermios *old)
{
	TRACE_L("set_termios");
}

/* Called without the kernel lock held - fine */
static unsigned int r3964_poll(struct tty_struct *tty, struct file *file,
			struct poll_table_struct *wait)
{
	struct r3964_info *pInfo = (struct r3964_info *)tty->disc_data;
	struct r3964_client_info *pClient;
	struct r3964_message *pMsg = NULL;
	unsigned long flags;
	int result = POLLOUT;

	TRACE_L("POLL");

	pClient = findClient(pInfo, task_pid(current));
	if (pClient) {
		poll_wait(file, &pInfo->read_wait, wait);
		spin_lock_irqsave(&pInfo->lock, flags);
		pMsg = pClient->first_msg;
		spin_unlock_irqrestore(&pInfo->lock, flags);
		if (pMsg)
			result |= POLLIN | POLLRDNORM;
	} else {
		result = -EINVAL;
	}
	return result;
}

static void r3964_receive_buf(struct tty_struct *tty, const unsigned char *cp,
			char *fp, int count)
{
	struct r3964_info *pInfo = (struct r3964_info *)tty->disc_data;
	const unsigned char *p;
	char *f, flags = 0;
	int i;

	for (i = count, p = cp, f = fp; i; i--, p++) {
		if (f)
			flags = *f++;
		if (flags == TTY_NORMAL) {
			receive_char(pInfo, *p);
		} else {
			receive_error(pInfo, flags);
		}

	}
}

MODULE_LICENSE("GPL");
MODULE_ALIAS_LDISC(N_R3964);