sg.c

来自「Linux Kernel 2.6.9 for OMAP1710」· C语言 代码 · 共 2,278 行 · 第 1/5 页

static int
sg_build_indirect(Sg_scatter_hold * schp, Sg_fd * sfp, int buff_size)
{
	int ret_sz;
	int blk_size = buff_size;
	unsigned char *p = NULL;

	if ((blk_size < 0) || (!sfp))
		return -EFAULT;
	if (0 == blk_size)
		++blk_size;	/* don't know why */
/* round request up to next highest SG_SECTOR_SZ byte boundary */
	blk_size = (blk_size + SG_SECTOR_MSK) & (~SG_SECTOR_MSK);
	SCSI_LOG_TIMEOUT(4, printk("sg_build_indirect: buff_size=%d, blk_size=%d\n",
				   buff_size, blk_size));
	if (blk_size <= SG_SCATTER_SZ) {
		p = sg_page_malloc(blk_size, sfp->low_dma, &ret_sz);
		if (!p)
			return -ENOMEM;
		if (blk_size == ret_sz) {	/* got it on the first attempt */
			schp->k_use_sg = 0;
			schp->buffer = p;
			schp->bufflen = blk_size;
			schp->b_malloc_len = blk_size;
			return 0;
		}
	} else {
		p = sg_page_malloc(SG_SCATTER_SZ, sfp->low_dma, &ret_sz);
		if (!p)
			return -ENOMEM;
	}
/* Want some local declarations, so start new block ... */
	{			/* lets try and build a scatter gather list */
		struct scatterlist *sclp;
		int k, rem_sz, num;
		int mx_sc_elems;
		int sg_tablesize = sfp->parentdp->sg_tablesize;
		int first = 1;

		/* N.B. ret_sz carried into this block ... */
		mx_sc_elems = sg_build_sgat(schp, sfp, sg_tablesize);
		if (mx_sc_elems < 0)
			return mx_sc_elems;	/* most likely -ENOMEM */

		for (k = 0, sclp = schp->buffer, rem_sz = blk_size;
		     (rem_sz > 0) && (k < mx_sc_elems);
		     ++k, rem_sz -= ret_sz, ++sclp) {
			if (first)
				first = 0;
			else {
				num =
				    (rem_sz >
				     SG_SCATTER_SZ) ? SG_SCATTER_SZ : rem_sz;
				p = sg_page_malloc(num, sfp->low_dma, &ret_sz);
				if (!p)
					break;
			}
			sclp->page = virt_to_page(p);
			sclp->offset = offset_in_page(p);
			sclp->length = ret_sz;

			SCSI_LOG_TIMEOUT(5, printk("sg_build_build: k=%d, a=0x%p, len=%d\n",
					  k, sg_scatg2virt(sclp), ret_sz));
		}		/* end of for loop */
		schp->k_use_sg = k;
		SCSI_LOG_TIMEOUT(5, printk("sg_build_indirect: k_use_sg=%d, rem_sz=%d\n", k, rem_sz));
		schp->bufflen = blk_size;
		if (rem_sz > 0)	/* must have failed */
			return -ENOMEM;
	}
	return 0;
}

static int
sg_write_xfer(Sg_request * srp)
{
	sg_io_hdr_t *hp = &srp->header;
	Sg_scatter_hold *schp = &srp->data;
	int num_xfer = 0;
	int j, k, onum, usglen, ksglen, res;
	int iovec_count = (int) hp->iovec_count;
	int dxfer_dir = hp->dxfer_direction;
	unsigned char *p;
	unsigned char __user *up;
	int new_interface = ('\0' == hp->interface_id) ? 0 : 1;

	if ((SG_DXFER_UNKNOWN == dxfer_dir) || (SG_DXFER_TO_DEV == dxfer_dir) ||
	    (SG_DXFER_TO_FROM_DEV == dxfer_dir)) {
		num_xfer = (int) (new_interface ? hp->dxfer_len : hp->flags);
		if (schp->bufflen < num_xfer)
			num_xfer = schp->bufflen;
	}
	if ((num_xfer <= 0) || (schp->dio_in_use) ||
	    (new_interface
	     && ((SG_FLAG_NO_DXFER | SG_FLAG_MMAP_IO) & hp->flags)))
		return 0;

	SCSI_LOG_TIMEOUT(4, printk("sg_write_xfer: num_xfer=%d, iovec_count=%d, k_use_sg=%d\n",
			  num_xfer, iovec_count, schp->k_use_sg));
	if (iovec_count) {
		onum = iovec_count;
		if ((k = verify_area(VERIFY_READ, hp->dxferp,
				     SZ_SG_IOVEC * onum)))
			return k;
	} else
		onum = 1;

	if (0 == schp->k_use_sg) {	/* kernel has single buffer */
		for (j = 0, p = schp->buffer; j < onum; ++j) {
			res = sg_u_iovec(hp, iovec_count, j, 1, &usglen, &up);
			if (res)
				return res;
			usglen = (num_xfer > usglen) ? usglen : num_xfer;
			if (__copy_from_user(p, up, usglen))
				return -EFAULT;
			p += usglen;
			num_xfer -= usglen;
			if (num_xfer <= 0)
				return 0;
		}
	} else {		/* kernel using scatter gather list */
		struct scatterlist *sclp = (struct scatterlist *) schp->buffer;

		ksglen = (int) sclp->length;
		p = sg_scatg2virt(sclp);
		for (j = 0, k = 0; j < onum; ++j) {
			res = sg_u_iovec(hp, iovec_count, j, 1, &usglen, &up);
			if (res)
				return res;

			for (; p; ++sclp, ksglen = (int) sclp->length,
				  p = sg_scatg2virt(sclp)) {
				if (usglen <= 0)
					break;
				if (ksglen > usglen) {
					if (usglen >= num_xfer) {
						if (__copy_from_user
						    (p, up, num_xfer))
							return -EFAULT;
						return 0;
					}
					if (__copy_from_user(p, up, usglen))
						return -EFAULT;
					p += usglen;
					ksglen -= usglen;
					break;
				} else {
					if (ksglen >= num_xfer) {
						if (__copy_from_user
						    (p, up, num_xfer))
							return -EFAULT;
						return 0;
					}
					if (__copy_from_user(p, up, ksglen))
						return -EFAULT;
					up += ksglen;
					usglen -= ksglen;
				}
				++k;
				if (k >= schp->k_use_sg)
					return 0;
			}
		}
	}
	return 0;
}

static int
sg_u_iovec(sg_io_hdr_t * hp, int sg_num, int ind,
	   int wr_xf, int *countp, unsigned char __user **up)
{
	int num_xfer = (int) hp->dxfer_len;
	unsigned char __user *p = hp->dxferp;
	int count, k;

	if (0 == sg_num) {
		if (wr_xf && ('\0' == hp->interface_id))
			count = (int) hp->flags;	/* holds "old" input_size */
		else
			count = num_xfer;
	} else {
		sg_iovec_t iovec;
		if (__copy_from_user(&iovec, p + ind*SZ_SG_IOVEC, SZ_SG_IOVEC))
			return -EFAULT;
		p = iovec.iov_base;
		count = (int) iovec.iov_len;
	}
	if ((k = verify_area(wr_xf ? VERIFY_READ : VERIFY_WRITE, p, count)))
		return k;
	if (up)
		*up = p;
	if (countp)
		*countp = count;
	return 0;
}

static void
sg_remove_scat(Sg_scatter_hold * schp)
{
	SCSI_LOG_TIMEOUT(4, printk("sg_remove_scat: k_use_sg=%d\n", schp->k_use_sg));
	if (schp->buffer && (schp->sglist_len > 0)) {
		struct scatterlist *sclp = (struct scatterlist *) schp->buffer;

		if (schp->dio_in_use) {
#ifdef SG_ALLOW_DIO_CODE
			st_unmap_user_pages(sclp, schp->k_use_sg, TRUE);
#endif
		} else {
			int k;

			for (k = 0; (k < schp->k_use_sg) && sg_scatg2virt(sclp);
			     ++k, ++sclp) {
				SCSI_LOG_TIMEOUT(5, printk(
				    "sg_remove_scat: k=%d, a=0x%p, len=%d\n",
				    k, sg_scatg2virt(sclp), sclp->length));
				sg_page_free(sg_scatg2virt(sclp), sclp->length);
				sclp->page = NULL;
				sclp->offset = 0;
				sclp->length = 0;
			}
		}
		sg_page_free(schp->buffer, schp->sglist_len);
	} else if (schp->buffer)
		sg_page_free(schp->buffer, schp->b_malloc_len);
	memset(schp, 0, sizeof (*schp));
}

static int
sg_read_xfer(Sg_request * srp)
{
	sg_io_hdr_t *hp = &srp->header;
	Sg_scatter_hold *schp = &srp->data;
	int num_xfer = 0;
	int j, k, onum, usglen, ksglen, res;
	int iovec_count = (int) hp->iovec_count;
	int dxfer_dir = hp->dxfer_direction;
	unsigned char *p;
	unsigned char __user *up;
	int new_interface = ('\0' == hp->interface_id) ? 0 : 1;

	if ((SG_DXFER_UNKNOWN == dxfer_dir) || (SG_DXFER_FROM_DEV == dxfer_dir)
	    || (SG_DXFER_TO_FROM_DEV == dxfer_dir)) {
		num_xfer = hp->dxfer_len;
		if (schp->bufflen < num_xfer)
			num_xfer = schp->bufflen;
	}
	if ((num_xfer <= 0) || (schp->dio_in_use) ||
	    (new_interface
	     && ((SG_FLAG_NO_DXFER | SG_FLAG_MMAP_IO) & hp->flags)))
		return 0;

	SCSI_LOG_TIMEOUT(4, printk("sg_read_xfer: num_xfer=%d, iovec_count=%d, k_use_sg=%d\n",
			  num_xfer, iovec_count, schp->k_use_sg));
	if (iovec_count) {
		onum = iovec_count;
		if ((k = verify_area(VERIFY_READ, hp->dxferp,
				     SZ_SG_IOVEC * onum)))
			return k;
	} else
		onum = 1;

	if (0 == schp->k_use_sg) {	/* kernel has single buffer */
		for (j = 0, p = schp->buffer; j < onum; ++j) {
			res = sg_u_iovec(hp, iovec_count, j, 0, &usglen, &up);
			if (res)
				return res;
			usglen = (num_xfer > usglen) ? usglen : num_xfer;
			if (__copy_to_user(up, p, usglen))
				return -EFAULT;
			p += usglen;
			num_xfer -= usglen;
			if (num_xfer <= 0)
				return 0;
		}
	} else {		/* kernel using scatter gather list */
		struct scatterlist *sclp = (struct scatterlist *) schp->buffer;

		ksglen = (int) sclp->length;
		p = sg_scatg2virt(sclp);
		for (j = 0, k = 0; j < onum; ++j) {
			res = sg_u_iovec(hp, iovec_count, j, 0, &usglen, &up);
			if (res)
				return res;

			for (; p; ++sclp, ksglen = (int) sclp->length,
				  p = sg_scatg2virt(sclp)) {
				if (usglen <= 0)
					break;
				if (ksglen > usglen) {
					if (usglen >= num_xfer) {
						if (__copy_to_user
						    (up, p, num_xfer))
							return -EFAULT;
						return 0;
					}
					if (__copy_to_user(up, p, usglen))
						return -EFAULT;
					p += usglen;
					ksglen -= usglen;
					break;
				} else {
					if (ksglen >= num_xfer) {
						if (__copy_to_user
						    (up, p, num_xfer))
							return -EFAULT;
						return 0;
					}
					if (__copy_to_user(up, p, ksglen))
						return -EFAULT;
					up += ksglen;
					usglen -= ksglen;
				}
				++k;
				if (k >= schp->k_use_sg)
					return 0;
			}
		}
	}
	return 0;
}

static int
sg_read_oxfer(Sg_request * srp, char __user *outp, int num_read_xfer)
{
	Sg_scatter_hold *schp = &srp->data;

	SCSI_LOG_TIMEOUT(4, printk("sg_read_oxfer: num_read_xfer=%d\n",
				   num_read_xfer));
	if ((!outp) || (num_read_xfer <= 0))
		return 0;
	if (schp->k_use_sg > 0) {
		int k, num;
		struct scatterlist *sclp = (struct scatterlist *) schp->buffer;

		for (k = 0; (k < schp->k_use_sg) && sg_scatg2virt(sclp);
		     ++k, ++sclp) {
			num = (int) sclp->length;
			if (num > num_read_xfer) {
				if (__copy_to_user
				    (outp, sg_scatg2virt(sclp), num_read_xfer))
					return -EFAULT;
				break;
			} else {
				if (__copy_to_user
				    (outp, sg_scatg2virt(sclp), num))
					return -EFAULT;
				num_read_xfer -= num;
				if (num_read_xfer <= 0)
					break;
				outp += num;
			}
		}
	} else {
		if (__copy_to_user(outp, schp->buffer, num_read_xfer))
			return -EFAULT;
	}
	return 0;
}

static void
sg_build_reserve(Sg_fd * sfp, int req_size)
{
	Sg_scatter_hold *schp = &sfp->reserve;

	SCSI_LOG_TIMEOUT(4, printk("sg_build_reserve: req_size=%d\n", req_size));
	do {
		if (req_size < PAGE_SIZE)
			req_size = PAGE_SIZE;
		if (0 == sg_build_indirect(schp, sfp, req_size))
			return;
		else
			sg_remove_scat(schp);
		req_size >>= 1;	/* divide by 2 */
	} while (req_size > (PAGE_SIZE / 2));
}

static void
sg_link_reserve(Sg_fd * sfp, Sg_request * srp, int size)
{
	Sg_scatter_hold *req_schp = &srp->data;
	Sg_scatter_hold *rsv_schp = &sfp->reserve;

	srp->res_used = 1;
	SCSI_LOG_TIMEOUT(4, printk("sg_link_reserve: size=%d\n", size));
	size = (size + 1) & (~1);	/* round to even for aha1542 */
	if (rsv_schp->k_use_sg > 0) {
		int k, num;
		int rem = size;
		struct scatterlist *sclp =
		    (struct scatterlist *) rsv_schp->buffer;

		for (k = 0; k < rsv_schp->k_use_sg; ++k, ++sclp) {
			num = (int) sclp->length;
			if (rem <= num) {
				if (0 == k) {
					req_schp->k_use_sg = 0;
					req_schp->buffer = sg_scatg2virt(sclp);
				} else {
					sfp->save_scat_len = num;
					sclp->length = (unsigned) rem;
					req_schp->k_use_sg = k + 1;
					req_schp->sglist_len =
					    rsv_schp->sglist_len;
					req_schp->buffer = rsv_schp->buffer;
				}
				req_schp->bufflen = size;
				req_schp->b_malloc_len = rsv_schp->b_malloc_len;
				break;
			} else
				rem -= num;
		}
		if (k >= rsv_schp->k_use_sg)
			SCSI_LOG_TIMEOUT(1, printk("sg_link_reserve: BAD size\n"));
	} else {
		req_schp->k_use_sg = 0;
		req_schp->bufflen = size;
		req_schp->buffer = rsv_schp->buffer;
		req_schp->b_malloc_len = rsv_schp->b_malloc_len;
	}
}

static void
sg_unlink_reserve(Sg_fd * sfp, Sg_request * srp)
{
	Sg_scatter_hold *req_schp = &srp->data;
	Sg_scatter_hold *rsv_schp = &sfp->reserve;

	SCSI_LOG_TIMEOUT(4, printk("sg_unlink_reserve: req->k_use_sg=%d\n",
				   (int) req_schp->k_use_sg));
	if ((rsv_schp->k_use_sg > 0) && (req_schp->k_use_sg > 0)) {
		struct scatterlist *sclp =
		    (struct scatterlist *) rsv_schp->buffer;

		if (sfp->save_scat_len > 0)
			(sclp + (req_schp->k_use_sg - 1))->length =
			    (unsigned) sfp->save_scat_len;
		else
			SCSI_LOG_TIMEOUT(1, printk ("sg_unlink_reserve: BAD save_scat_len\n"));
	}
	req_schp->k_use_sg = 0;
	req_schp->bufflen = 0;
	req_schp->buffer = NULL;
	req_schp->sglist_len = 0;
	sfp->save_scat_len = 0;
	srp->res_used = 0;
}

static Sg_request *
sg_get_rq_mark(Sg_fd * sfp, int pack_id)
{
	Sg_request *resp;
	unsigned long iflags;

	write_