drivers/staging/media/atomisp/pci/atomisp2/css2400/hive_isp_css_common/host/vmem.c

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