1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  * Support for Intel Camera Imaging ISP subsystem.
4  * Copyright (c) 2010 - 2016, Intel Corporation.
5  *
6  * This program is free software; you can redistribute it and/or modify it
7  * under the terms and conditions of the GNU General Public License,
8  * version 2, as published by the Free Software Foundation.
9  *
10  * This program is distributed in the hope it will be useful, but WITHOUT
11  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
12  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
13  * more details.
14  */
15 
16 #include "isp.h"
17 #include "vmem.h"
18 #include "vmem_local.h"
19 
20 #if !defined(HRT_MEMORY_ACCESS)
21 #include "ia_css_device_access.h"
22 #endif
23 #include "assert_support.h"
24 
25 typedef unsigned long long hive_uedge;
26 typedef hive_uedge *hive_wide;
27 
28 /* Copied from SDK: sim_semantics.c */
29 
30 /* subword bits move like this:         MSB[____xxxx____]LSB -> MSB[00000000xxxx]LSB */
31 static inline hive_uedge
subword(hive_uedge w,unsigned int start,unsigned int end)32 subword(hive_uedge w, unsigned int start, unsigned int end)
33 {
34 	return (w & (((1ULL << (end - 1)) - 1) << 1 | 1)) >> start;
35 }
36 
37 /* inverse subword bits move like this: MSB[xxxx____xxxx]LSB -> MSB[xxxx0000xxxx]LSB */
38 static inline hive_uedge
inv_subword(hive_uedge w,unsigned int start,unsigned int end)39 inv_subword(hive_uedge w, unsigned int start, unsigned int end)
40 {
41 	return w & (~(((1ULL << (end - 1)) - 1) << 1 | 1) | ((1ULL << start) - 1));
42 }
43 
44 #define uedge_bits (8 * sizeof(hive_uedge))
45 #define move_lower_bits(target, target_bit, src, src_bit) move_subword(target, target_bit, src, 0, src_bit)
46 #define move_upper_bits(target, target_bit, src, src_bit) move_subword(target, target_bit, src, src_bit, uedge_bits)
47 #define move_word(target, target_bit, src) move_subword(target, target_bit, src, 0, uedge_bits)
48 
49 static void
move_subword(hive_uedge * target,unsigned int target_bit,hive_uedge src,unsigned int src_start,unsigned int src_end)50 move_subword(
51     hive_uedge *target,
52     unsigned int target_bit,
53     hive_uedge src,
54     unsigned int src_start,
55     unsigned int src_end)
56 {
57 	unsigned int start_elem = target_bit / uedge_bits;
58 	unsigned int start_bit  = target_bit % uedge_bits;
59 	unsigned int subword_width = src_end - src_start;
60 
61 	hive_uedge src_subword = subword(src, src_start, src_end);
62 
63 	if (subword_width + start_bit > uedge_bits) { /* overlap */
64 		hive_uedge old_val1;
65 		hive_uedge old_val0 = inv_subword(target[start_elem], start_bit, uedge_bits);
66 
67 		target[start_elem] = old_val0 | (src_subword << start_bit);
68 		old_val1 = inv_subword(target[start_elem + 1], 0,
69 				       subword_width + start_bit - uedge_bits);
70 		target[start_elem + 1] = old_val1 | (src_subword >> (uedge_bits - start_bit));
71 	} else {
72 		hive_uedge old_val = inv_subword(target[start_elem], start_bit,
73 						 start_bit + subword_width);
74 
75 		target[start_elem] = old_val | (src_subword << start_bit);
76 	}
77 }
78 
79 static void
hive_sim_wide_unpack(hive_wide vector,hive_wide elem,hive_uint elem_bits,hive_uint index)80 hive_sim_wide_unpack(
81     hive_wide vector,
82     hive_wide elem,
83     hive_uint elem_bits,
84     hive_uint index)
85 {
86 	/* pointers into wide_type: */
87 	unsigned int start_elem = (elem_bits * index) / uedge_bits;
88 	unsigned int start_bit  = (elem_bits * index) % uedge_bits;
89 	unsigned int end_elem   = (elem_bits * (index + 1) - 1) / uedge_bits;
90 	unsigned int end_bit    = ((elem_bits * (index + 1) - 1) % uedge_bits) + 1;
91 
92 	if (elem_bits == uedge_bits) {
93 		/* easy case for speedup: */
94 		elem[0] = vector[index];
95 	} else if (start_elem == end_elem) {
96 		/* only one (<=64 bits) element needs to be (partly) copied: */
97 		move_subword(elem, 0, vector[start_elem], start_bit, end_bit);
98 	} else {
99 		/* general case: handles edge spanning cases (includes >64bit elements) */
100 		unsigned int bits_written = 0;
101 		unsigned int i;
102 
103 		move_upper_bits(elem, bits_written, vector[start_elem], start_bit);
104 		bits_written += (64 - start_bit);
105 		for (i = start_elem + 1; i < end_elem; i++) {
106 			move_word(elem, bits_written, vector[i]);
107 			bits_written += uedge_bits;
108 		}
109 		move_lower_bits(elem, bits_written, vector[end_elem], end_bit);
110 	}
111 }
112 
113 static void
hive_sim_wide_pack(hive_wide vector,hive_wide elem,hive_uint elem_bits,hive_uint index)114 hive_sim_wide_pack(
115     hive_wide vector,
116     hive_wide elem,
117     hive_uint elem_bits,
118     hive_uint index)
119 {
120 	/* pointers into wide_type: */
121 	unsigned int start_elem = (elem_bits * index) / uedge_bits;
122 
123 	/* easy case for speedup: */
124 	if (elem_bits == uedge_bits) {
125 		vector[start_elem] = elem[0];
126 	} else if (elem_bits > uedge_bits) {
127 		unsigned int bits_to_write = elem_bits;
128 		unsigned int start_bit = elem_bits * index;
129 		unsigned int i = 0;
130 
131 		for (; bits_to_write > uedge_bits;
132 		     bits_to_write -= uedge_bits, i++, start_bit += uedge_bits) {
133 			move_word(vector, start_bit, elem[i]);
134 		}
135 		move_lower_bits(vector, start_bit, elem[i], bits_to_write);
136 	} else {
137 		/* only one element needs to be (partly) copied: */
138 		move_lower_bits(vector, elem_bits * index, elem[0], elem_bits);
139 	}
140 }
141 
load_vector(const isp_ID_t ID,t_vmem_elem * to,const t_vmem_elem * from)142 static void load_vector(
143     const isp_ID_t		ID,
144     t_vmem_elem		*to,
145     const t_vmem_elem	*from)
146 {
147 	unsigned int i;
148 	hive_uedge *data;
149 	unsigned int size = sizeof(short) * ISP_NWAY;
150 
151 	VMEM_ARRAY(v, 2 * ISP_NWAY); /* Need 2 vectors to work around vmem hss bug */
152 	assert(ISP_BAMEM_BASE[ID] != (hrt_address) - 1);
153 #if !defined(HRT_MEMORY_ACCESS)
154 	ia_css_device_load(ISP_BAMEM_BASE[ID] + (unsigned long)from, &v[0][0], size);
155 #else
156 	hrt_master_port_load(ISP_BAMEM_BASE[ID] + (unsigned long)from, &v[0][0], size);
157 #endif
158 	data = (hive_uedge *)v;
159 	for (i = 0; i < ISP_NWAY; i++) {
160 		hive_uedge elem = 0;
161 
162 		hive_sim_wide_unpack(data, &elem, ISP_VEC_ELEMBITS, i);
163 		to[i] = elem;
164 	}
165 	udelay(1); /* Spend at least 1 cycles per vector */
166 }
167 
store_vector(const isp_ID_t ID,t_vmem_elem * to,const t_vmem_elem * from)168 static void store_vector(
169     const isp_ID_t		ID,
170     t_vmem_elem		*to,
171     const t_vmem_elem	*from)
172 {
173 	unsigned int i;
174 	unsigned int size = sizeof(short) * ISP_NWAY;
175 
176 	VMEM_ARRAY(v, 2 * ISP_NWAY); /* Need 2 vectors to work around vmem hss bug */
177 	//load_vector (&v[1][0], &to[ISP_NWAY]); /* Fetch the next vector, since it will be overwritten. */
178 	hive_uedge *data = (hive_uedge *)v;
179 
180 	for (i = 0; i < ISP_NWAY; i++) {
181 		hive_sim_wide_pack(data, (hive_wide)&from[i], ISP_VEC_ELEMBITS, i);
182 	}
183 	assert(ISP_BAMEM_BASE[ID] != (hrt_address) - 1);
184 #if !defined(HRT_MEMORY_ACCESS)
185 	ia_css_device_store(ISP_BAMEM_BASE[ID] + (unsigned long)to, &v, size);
186 #else
187 	//hrt_mem_store (ISP, VMEM, (unsigned)to, &v, siz); /* This will overwrite the next vector as well */
188 	hrt_master_port_store(ISP_BAMEM_BASE[ID] + (unsigned long)to, &v, size);
189 #endif
190 	udelay(1); /* Spend at least 1 cycles per vector */
191 }
192 
isp_vmem_load(const isp_ID_t ID,const t_vmem_elem * from,t_vmem_elem * to,unsigned int elems)193 void isp_vmem_load(
194     const isp_ID_t		ID,
195     const t_vmem_elem	*from,
196     t_vmem_elem		*to,
197     unsigned int elems) /* In t_vmem_elem */
198 {
199 	unsigned int c;
200 	const t_vmem_elem *vp = from;
201 
202 	assert(ID < N_ISP_ID);
203 	assert((unsigned long)from % ISP_VEC_ALIGN == 0);
204 	assert(elems % ISP_NWAY == 0);
205 	for (c = 0; c < elems; c += ISP_NWAY) {
206 		load_vector(ID, &to[c], vp);
207 		vp = (t_vmem_elem *)((char *)vp + ISP_VEC_ALIGN);
208 	}
209 }
210 
isp_vmem_store(const isp_ID_t ID,t_vmem_elem * to,const t_vmem_elem * from,unsigned int elems)211 void isp_vmem_store(
212     const isp_ID_t		ID,
213     t_vmem_elem		*to,
214     const t_vmem_elem	*from,
215     unsigned int elems) /* In t_vmem_elem */
216 {
217 	unsigned int c;
218 	t_vmem_elem *vp = to;
219 
220 	assert(ID < N_ISP_ID);
221 	assert((unsigned long)to % ISP_VEC_ALIGN == 0);
222 	assert(elems % ISP_NWAY == 0);
223 	for (c = 0; c < elems; c += ISP_NWAY) {
224 		store_vector(ID, vp, &from[c]);
225 		vp = (t_vmem_elem *)((char *)vp + ISP_VEC_ALIGN);
226 	}
227 }
228 
isp_vmem_2d_load(const isp_ID_t ID,const t_vmem_elem * from,t_vmem_elem * to,unsigned int height,unsigned int width,unsigned int stride_to,unsigned stride_from)229 void isp_vmem_2d_load(
230     const isp_ID_t		ID,
231     const t_vmem_elem	*from,
232     t_vmem_elem		*to,
233     unsigned int height,
234     unsigned int width,
235     unsigned int stride_to,  /* In t_vmem_elem */
236 
237     unsigned stride_from /* In t_vmem_elem */)
238 {
239 	unsigned int h;
240 
241 	assert(ID < N_ISP_ID);
242 	assert((unsigned long)from % ISP_VEC_ALIGN == 0);
243 	assert(width % ISP_NWAY == 0);
244 	assert(stride_from % ISP_NWAY == 0);
245 	for (h = 0; h < height; h++) {
246 		unsigned int c;
247 		const t_vmem_elem *vp = from;
248 
249 		for (c = 0; c < width; c += ISP_NWAY) {
250 			load_vector(ID, &to[stride_to * h + c], vp);
251 			vp = (t_vmem_elem *)((char *)vp + ISP_VEC_ALIGN);
252 		}
253 		from = (const t_vmem_elem *)((const char *)from + stride_from / ISP_NWAY *
254 					     ISP_VEC_ALIGN);
255 	}
256 }
257 
isp_vmem_2d_store(const isp_ID_t ID,t_vmem_elem * to,const t_vmem_elem * from,unsigned int height,unsigned int width,unsigned int stride_to,unsigned stride_from)258 void isp_vmem_2d_store(
259     const isp_ID_t		ID,
260     t_vmem_elem		*to,
261     const t_vmem_elem	*from,
262     unsigned int height,
263     unsigned int width,
264     unsigned int stride_to,  /* In t_vmem_elem */
265 
266     unsigned stride_from /* In t_vmem_elem */)
267 {
268 	unsigned int h;
269 
270 	assert(ID < N_ISP_ID);
271 	assert((unsigned long)to % ISP_VEC_ALIGN == 0);
272 	assert(width % ISP_NWAY == 0);
273 	assert(stride_to % ISP_NWAY == 0);
274 	for (h = 0; h < height; h++) {
275 		unsigned int c;
276 		t_vmem_elem *vp = to;
277 
278 		for (c = 0; c < width; c += ISP_NWAY) {
279 			store_vector(ID, vp, &from[stride_from * h + c]);
280 			vp = (t_vmem_elem *)((char *)vp + ISP_VEC_ALIGN);
281 		}
282 		to = (t_vmem_elem *)((char *)to + stride_to / ISP_NWAY * ISP_VEC_ALIGN);
283 	}
284 }
285