1 /*
2  * Copyright (C) 2015 Robert Jarzmik <robert.jarzmik@free.fr>
3  *
4  * Scatterlist splitting helpers.
5  *
6  * This source code is licensed under the GNU General Public License,
7  * Version 2. See the file COPYING for more details.
8  */
9 
10 #include <linux/scatterlist.h>
11 #include <linux/slab.h>
12 
13 struct sg_splitter {
14 	struct scatterlist *in_sg0;
15 	int nents;
16 	off_t skip_sg0;
17 	unsigned int length_last_sg;
18 
19 	struct scatterlist *out_sg;
20 };
21 
sg_calculate_split(struct scatterlist * in,int nents,int nb_splits,off_t skip,const size_t * sizes,struct sg_splitter * splitters,bool mapped)22 static int sg_calculate_split(struct scatterlist *in, int nents, int nb_splits,
23 			      off_t skip, const size_t *sizes,
24 			      struct sg_splitter *splitters, bool mapped)
25 {
26 	int i;
27 	unsigned int sglen;
28 	size_t size = sizes[0], len;
29 	struct sg_splitter *curr = splitters;
30 	struct scatterlist *sg;
31 
32 	for (i = 0; i < nb_splits; i++) {
33 		splitters[i].in_sg0 = NULL;
34 		splitters[i].nents = 0;
35 	}
36 
37 	for_each_sg(in, sg, nents, i) {
38 		sglen = mapped ? sg_dma_len(sg) : sg->length;
39 		if (skip > sglen) {
40 			skip -= sglen;
41 			continue;
42 		}
43 
44 		len = min_t(size_t, size, sglen - skip);
45 		if (!curr->in_sg0) {
46 			curr->in_sg0 = sg;
47 			curr->skip_sg0 = skip;
48 		}
49 		size -= len;
50 		curr->nents++;
51 		curr->length_last_sg = len;
52 
53 		while (!size && (skip + len < sglen) && (--nb_splits > 0)) {
54 			curr++;
55 			size = *(++sizes);
56 			skip += len;
57 			len = min_t(size_t, size, sglen - skip);
58 
59 			curr->in_sg0 = sg;
60 			curr->skip_sg0 = skip;
61 			curr->nents = 1;
62 			curr->length_last_sg = len;
63 			size -= len;
64 		}
65 		skip = 0;
66 
67 		if (!size && --nb_splits > 0) {
68 			curr++;
69 			size = *(++sizes);
70 		}
71 
72 		if (!nb_splits)
73 			break;
74 	}
75 
76 	return (size || !splitters[0].in_sg0) ? -EINVAL : 0;
77 }
78 
sg_split_phys(struct sg_splitter * splitters,const int nb_splits)79 static void sg_split_phys(struct sg_splitter *splitters, const int nb_splits)
80 {
81 	int i, j;
82 	struct scatterlist *in_sg, *out_sg;
83 	struct sg_splitter *split;
84 
85 	for (i = 0, split = splitters; i < nb_splits; i++, split++) {
86 		in_sg = split->in_sg0;
87 		out_sg = split->out_sg;
88 		for (j = 0; j < split->nents; j++, out_sg++) {
89 			*out_sg = *in_sg;
90 			if (!j) {
91 				out_sg->offset += split->skip_sg0;
92 				out_sg->length -= split->skip_sg0;
93 			} else {
94 				out_sg->offset = 0;
95 			}
96 			sg_dma_address(out_sg) = 0;
97 			sg_dma_len(out_sg) = 0;
98 			in_sg = sg_next(in_sg);
99 		}
100 		out_sg[-1].length = split->length_last_sg;
101 		sg_mark_end(out_sg - 1);
102 	}
103 }
104 
sg_split_mapped(struct sg_splitter * splitters,const int nb_splits)105 static void sg_split_mapped(struct sg_splitter *splitters, const int nb_splits)
106 {
107 	int i, j;
108 	struct scatterlist *in_sg, *out_sg;
109 	struct sg_splitter *split;
110 
111 	for (i = 0, split = splitters; i < nb_splits; i++, split++) {
112 		in_sg = split->in_sg0;
113 		out_sg = split->out_sg;
114 		for (j = 0; j < split->nents; j++, out_sg++) {
115 			sg_dma_address(out_sg) = sg_dma_address(in_sg);
116 			sg_dma_len(out_sg) = sg_dma_len(in_sg);
117 			if (!j) {
118 				sg_dma_address(out_sg) += split->skip_sg0;
119 				sg_dma_len(out_sg) -= split->skip_sg0;
120 			}
121 			in_sg = sg_next(in_sg);
122 		}
123 		sg_dma_len(--out_sg) = split->length_last_sg;
124 	}
125 }
126 
127 /**
128  * sg_split - split a scatterlist into several scatterlists
129  * @in: the input sg list
130  * @in_mapped_nents: the result of a dma_map_sg(in, ...), or 0 if not mapped.
131  * @skip: the number of bytes to skip in the input sg list
132  * @nb_splits: the number of desired sg outputs
133  * @split_sizes: the respective size of each output sg list in bytes
134  * @out: an array where to store the allocated output sg lists
135  * @out_mapped_nents: the resulting sg lists mapped number of sg entries. Might
136  *                    be NULL if sglist not already mapped (in_mapped_nents = 0)
137  * @gfp_mask: the allocation flag
138  *
139  * This function splits the input sg list into nb_splits sg lists, which are
140  * allocated and stored into out.
141  * The @in is split into :
142  *  - @out[0], which covers bytes [@skip .. @skip + @split_sizes[0] - 1] of @in
143  *  - @out[1], which covers bytes [@skip + split_sizes[0] ..
144  *                                 @skip + @split_sizes[0] + @split_sizes[1] -1]
145  * etc ...
146  * It will be the caller's duty to kfree() out array members.
147  *
148  * Returns 0 upon success, or error code
149  */
sg_split(struct scatterlist * in,const int in_mapped_nents,const off_t skip,const int nb_splits,const size_t * split_sizes,struct scatterlist ** out,int * out_mapped_nents,gfp_t gfp_mask)150 int sg_split(struct scatterlist *in, const int in_mapped_nents,
151 	     const off_t skip, const int nb_splits,
152 	     const size_t *split_sizes,
153 	     struct scatterlist **out, int *out_mapped_nents,
154 	     gfp_t gfp_mask)
155 {
156 	int i, ret;
157 	struct sg_splitter *splitters;
158 
159 	splitters = kcalloc(nb_splits, sizeof(*splitters), gfp_mask);
160 	if (!splitters)
161 		return -ENOMEM;
162 
163 	ret = sg_calculate_split(in, sg_nents(in), nb_splits, skip, split_sizes,
164 			   splitters, false);
165 	if (ret < 0)
166 		goto err;
167 
168 	ret = -ENOMEM;
169 	for (i = 0; i < nb_splits; i++) {
170 		splitters[i].out_sg = kmalloc_array(splitters[i].nents,
171 						    sizeof(struct scatterlist),
172 						    gfp_mask);
173 		if (!splitters[i].out_sg)
174 			goto err;
175 	}
176 
177 	/*
178 	 * The order of these 3 calls is important and should be kept.
179 	 */
180 	sg_split_phys(splitters, nb_splits);
181 	ret = sg_calculate_split(in, in_mapped_nents, nb_splits, skip,
182 				 split_sizes, splitters, true);
183 	if (ret < 0)
184 		goto err;
185 	sg_split_mapped(splitters, nb_splits);
186 
187 	for (i = 0; i < nb_splits; i++) {
188 		out[i] = splitters[i].out_sg;
189 		if (out_mapped_nents)
190 			out_mapped_nents[i] = splitters[i].nents;
191 	}
192 
193 	kfree(splitters);
194 	return 0;
195 
196 err:
197 	for (i = 0; i < nb_splits; i++)
198 		kfree(splitters[i].out_sg);
199 	kfree(splitters);
200 	return ret;
201 }
202 EXPORT_SYMBOL(sg_split);
203