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
13struct 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
22static 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
79static 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
105static 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 */
150int 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
196err:
197	for (i = 0; i < nb_splits; i++)
198		kfree(splitters[i].out_sg);
199	kfree(splitters);
200	return ret;
201}
202EXPORT_SYMBOL(sg_split);
203