Orange Pi5 kernel

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