sg.c

linux和2410结合开发 用他可以生成2410所需的zImage文件

	(dxfer_dir != SG_DXFER_UNKNOWN) && (0 == hp->iovec_count) &&
	(! sfp->parentdp->device->host->unchecked_isa_dma)) {
	res = sg_build_dir(srp, sfp, dxfer_len);
	if (res <= 0)   /* -ve -> error, 0 -> done, 1 -> try indirect */
	    return res;
    }
    if ((! sg_res_in_use(sfp)) && (dxfer_len <= rsv_schp->bufflen))
	sg_link_reserve(sfp, srp, dxfer_len);
    else {
	res = sg_build_indi(req_schp, sfp, dxfer_len);
        if (res) {
            sg_remove_scat(req_schp);
            return res;
        }
    }
    return 0;
}

static void sg_finish_rem_req(Sg_request * srp)
{
    Sg_fd * sfp = srp->parentfp;
    Sg_scatter_hold * req_schp = &srp->data;

    SCSI_LOG_TIMEOUT(4, printk("sg_finish_rem_req: res_used=%d\n",
			       (int)srp->res_used));
    sg_unmap_and(&srp->data, 1);
    if (srp->res_used)
        sg_unlink_reserve(sfp, srp);
    else
        sg_remove_scat(req_schp);
    sg_remove_request(sfp, srp);
}

static int sg_build_sgat(Sg_scatter_hold * schp, const Sg_fd * sfp,
			 int tablesize)
{
    int mem_src, ret_sz;
    int elem_sz = sizeof(struct scatterlist) + sizeof(char);
    /* scatter gather array, followed by mem_src_arr (array of chars) */
    int sg_bufflen = tablesize * elem_sz;
    int mx_sc_elems = tablesize;

    mem_src = SG_HEAP_KMAL;
    schp->buffer = sg_malloc(sfp, sg_bufflen, &ret_sz, &mem_src);
    if (! schp->buffer)
	return -ENOMEM;
    else if (ret_sz != sg_bufflen) {
	sg_bufflen = ret_sz;
	mx_sc_elems = sg_bufflen / elem_sz;
    }
    schp->buffer_mem_src = (char)mem_src;
    schp->sglist_len = sg_bufflen;
    memset(schp->buffer, 0, sg_bufflen);
    return mx_sc_elems; /* number of scat_gath elements allocated */
}

static void sg_unmap_and(Sg_scatter_hold * schp, int free_also)
{
#ifdef SG_ALLOW_DIO_CODE
    int nbhs = 0;

    if (schp && schp->kiobp) {
	if (schp->mapped) {
	    unmap_kiobuf(schp->kiobp);
	    schp->mapped = 0;
	}
	if (free_also) {
	    sg_free_kiovec(1, &schp->kiobp, &nbhs);
	    schp->kiobp = NULL;
	}
    }
#endif
}

static int sg_build_dir(Sg_request * srp, Sg_fd * sfp, int dxfer_len)
{
#ifdef SG_ALLOW_DIO_CODE
    int res, k, split, offset, num, mx_sc_elems, rem_sz;
    struct kiobuf * kp;
    char * mem_src_arr;
    struct scatterlist * sclp;
    unsigned long addr, prev_addr;
    sg_io_hdr_t * hp = &srp->header;
    Sg_scatter_hold * schp = &srp->data;
    int sg_tablesize = sfp->parentdp->sg_tablesize;
    int nbhs = 0;

    res = sg_alloc_kiovec(1, &schp->kiobp, &nbhs);
    if (0 != res) {
	SCSI_LOG_TIMEOUT(5, printk("sg_build_dir: sg_alloc_kiovec res=%d\n", 
			 res));
	return 1;
    }
    res = map_user_kiobuf((SG_DXFER_TO_DEV == hp->dxfer_direction) ? 1 : 0,
			  schp->kiobp, (unsigned long)hp->dxferp, dxfer_len);
    if (0 != res) {
	SCSI_LOG_TIMEOUT(5,
		printk("sg_build_dir: map_user_kiobuf res=%d\n", res));
	sg_unmap_and(schp, 1);
	return 1;
    }
    schp->mapped = 1;
    kp = schp->kiobp;
    prev_addr = (unsigned long) page_address(kp->maplist[0]);
    for (k = 1, split = 0; k < kp->nr_pages; ++k, prev_addr = addr) {
	addr = (unsigned long) page_address(kp->maplist[k]);
	if ((prev_addr + PAGE_SIZE) != addr) {
	    split = k;
	    break;
	}
    }
    if (! split) {
	schp->k_use_sg = 0;
	schp->buffer = page_address(kp->maplist[0]) + kp->offset;
	schp->bufflen = dxfer_len;
	schp->buffer_mem_src = SG_USER_MEM;
	schp->b_malloc_len = dxfer_len;
	hp->info |= SG_INFO_DIRECT_IO;
	return 0;
    }
    mx_sc_elems = sg_build_sgat(schp, sfp, sg_tablesize);
    if (mx_sc_elems <= 1) {
	sg_unmap_and(schp, 1);
	sg_remove_scat(schp);
	return 1;
    }
    mem_src_arr = schp->buffer + (mx_sc_elems * sizeof(struct scatterlist));
    for (k = 0, sclp = schp->buffer, rem_sz = dxfer_len;
	 (rem_sz > 0) && (k < mx_sc_elems);
	 ++k, ++sclp) {
	offset = (0 == k) ? kp->offset : 0;
	num = (rem_sz > (PAGE_SIZE - offset)) ? (PAGE_SIZE - offset) :
						rem_sz;
	sclp->address = page_address(kp->maplist[k]) + offset;
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,13)
	sclp->page = NULL;
#endif
	sclp->length = num;
	mem_src_arr[k] = SG_USER_MEM;
	rem_sz -= num;
	SCSI_LOG_TIMEOUT(5,
	    printk("sg_build_dir: k=%d, a=0x%p, len=%d, ms=%d\n",
	    k, sclp->address, num, mem_src_arr[k]));
    }
    schp->k_use_sg = k;
    SCSI_LOG_TIMEOUT(5,
	printk("sg_build_dir: k_use_sg=%d, rem_sz=%d\n", k, rem_sz));
    schp->bufflen = dxfer_len;
    if (rem_sz > 0) {   /* must have failed */
	sg_unmap_and(schp, 1);
	sg_remove_scat(schp);
	return 1;   /* out of scatter gather elements, try indirect */
    }
    hp->info |= SG_INFO_DIRECT_IO;
    return 0;
#else
    return 1;
#endif /* SG_ALLOW_DIO_CODE */
}

static int sg_build_indi(Sg_scatter_hold * schp, Sg_fd * sfp, int buff_size)
{
    int ret_sz, mem_src;
    int blk_size = buff_size;
    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_indi: buff_size=%d, blk_size=%d\n",
                               buff_size, blk_size));
    if (blk_size <= SG_SCATTER_SZ) {
        mem_src = SG_HEAP_PAGE;
        p = sg_malloc(sfp, blk_size, &ret_sz, &mem_src);
        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->buffer_mem_src = (char)mem_src;
            schp->b_malloc_len = blk_size;
            return 0;
        }
    }
    else {
        mem_src = SG_HEAP_PAGE;
        p = sg_malloc(sfp, SG_SCATTER_SZ, &ret_sz, &mem_src);
        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;
	char * mem_src_arr;

        /* N.B. ret_sz and mem_src 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 */
	mem_src_arr = schp->buffer +
		      (mx_sc_elems * sizeof(struct scatterlist));

	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;
                mem_src = SG_HEAP_PAGE;
                p = sg_malloc(sfp, num, &ret_sz, &mem_src);
                if (! p)
                    break;
            }
            sclp->address = p;
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,13)
	    sclp->page = NULL;
#endif
            sclp->length = ret_sz;
	    mem_src_arr[k] = mem_src;

	    SCSI_LOG_TIMEOUT(5,
		printk("sg_build_build: k=%d, a=0x%p, len=%d, ms=%d\n",
                k, sclp->address, ret_sz, mem_src));
        } /* end of for loop */
	schp->k_use_sg = k;
	SCSI_LOG_TIMEOUT(5,
	    printk("sg_build_indi: 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, ok;
    int iovec_count = (int)hp->iovec_count;
    int dxfer_dir = hp->dxfer_direction;
    unsigned char * p;
    unsigned char * 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) || 
    	(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 */
	if (SG_USER_MEM != schp->buffer_mem_src) { /* else nothing to do */

	    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;
		__copy_from_user(p, up, usglen);
		p += usglen;
		num_xfer -= usglen;
		if (num_xfer <= 0)
		    return 0;
            }
	}
    }
    else {      /* kernel using scatter gather list */
	struct scatterlist * sclp = (struct scatterlist *)schp->buffer;
	char * mem_src_arr = sg_get_sgat_msa(schp);
	ksglen = (int)sclp->length;
	p = sclp->address;

	for (j = 0, k = 0; j < onum; ++j) {
	    res = sg_u_iovec(hp, iovec_count, j, 1, &usglen, &up);
	    if (res) return res;

	    for (; (k < schp->k_use_sg) && p;
		 ++k, ++sclp, ksglen = (int)sclp->length, p = sclp->address) {
		ok = (SG_USER_MEM != mem_src_arr[k]);
		if (usglen <= 0)
		    break;
		if (ksglen > usglen) {
		    if (usglen >= num_xfer) {
			if (ok) __copy_from_user(p, up, num_xfer);
			return 0;
		    }
		    if (ok) __copy_from_user(p, up, usglen);
		    p += usglen;
		    ksglen -= usglen;
                    break;
		}
		else {
		    if (ksglen >= num_xfer) {
			if (ok) __copy_from_user(p, up, num_xfer);
			return 0;
		    }
		    if (ok) __copy_from_user(p, up, ksglen);
		    up += ksglen;
		    usglen -= ksglen;
		}
            }
        }
    }
    return 0;
}

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

    if (0 == sg_num) {
	p = (unsigned char *)hp->dxferp;
	if (wr_xf && ('\0' == hp->interface_id))
	    count = (int)hp->flags; /* holds "old" input_size */
	else
	    count = num_xfer;
    }
    else {
	__copy_from_user(&u_iovec,
			 (unsigned char *)hp->dxferp + (ind * SZ_SG_IOVEC),
			 SZ_SG_IOVEC);
	p = (unsigned char *)u_iovec.iov_base;
	count = (int)u_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 char * sg_get_sgat_msa(Sg_scatter_hold * schp)
{
    int elem_sz = sizeof(struct scatterlist) + sizeof(char);
    int mx_sc_elems = schp->sglist_len / elem_sz;
    return schp->buffer + (sizeof(struct scatterlist) * mx_sc_elems);
}

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) {
        int k, mem_src;
        struct scatterlist * sclp = (struct scatterlist *)schp->buffer;
	char * mem_src_arr = sg_get_sgat_msa(schp);

	for (k = 0; (k < schp->k_use_sg) && sclp->address; ++k, ++sclp) {
	    mem_src = mem_src_arr[k];
	    SCSI_LOG_TIMEOUT(5,
		printk("sg_remove_scat: k=%d, a=0x%p, len=%d, ms=%d\n",
                       k, sclp->address, sclp->length, mem_src));
            sg_free(sclp->address, sclp->length, mem_src);
            sclp->address = NULL;
#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,4,13)
	    sclp->page = NULL;
#endif
            sclp->length = 0;
        }
	sg_free(schp->buffer, schp->sglist_len, schp->buffer_mem_src);
    }
    else if (schp->buffer)
	sg_free(schp->buffer, schp->b_malloc_len, schp->buffer_mem_src);
    memset(sch