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 25typedef unsigned long long hive_uedge; 26typedef 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 */ 31static inline hive_uedge 32subword(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 */ 38static inline hive_uedge 39inv_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 49static void 50move_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 79static void 80hive_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 113static void 114hive_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 142static 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 168static 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 193void 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 211void 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 229void 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 258void 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