1 /*
2 * Copyright (c) 2015, Xilinx Inc. and Contributors. All rights reserved.
3 *
4 * SPDX-License-Identifier: BSD-3-Clause
5 */
6
7 #include <metal/errno.h>
8 #include <limits.h>
9 #include <metal/io.h>
10 #include <metal/sys.h>
11
metal_io_init(struct metal_io_region * io,void * virt,const metal_phys_addr_t * physmap,size_t size,unsigned page_shift,unsigned int mem_flags,const struct metal_io_ops * ops)12 void metal_io_init(struct metal_io_region *io, void *virt,
13 const metal_phys_addr_t *physmap, size_t size,
14 unsigned page_shift, unsigned int mem_flags,
15 const struct metal_io_ops *ops)
16 {
17 const struct metal_io_ops nops = {NULL, NULL, NULL, NULL, NULL, NULL};
18
19 io->virt = virt;
20 io->physmap = physmap;
21 io->size = size;
22 io->page_shift = page_shift;
23 if (page_shift >= sizeof(io->page_mask) * CHAR_BIT)
24 /* avoid overflow */
25 io->page_mask = -1UL;
26 else
27 io->page_mask = (1UL << page_shift) - 1UL;
28 io->mem_flags = mem_flags;
29 io->ops = ops ? *ops : nops;
30 metal_sys_io_mem_map(io);
31 }
32
metal_io_block_read(struct metal_io_region * io,unsigned long offset,void * restrict dst,int len)33 int metal_io_block_read(struct metal_io_region *io, unsigned long offset,
34 void *restrict dst, int len)
35 {
36 unsigned char *ptr = metal_io_virt(io, offset);
37 unsigned char *dest = dst;
38 int retlen;
39
40 if (offset > io->size)
41 return -ERANGE;
42 if ((offset + len) > io->size)
43 len = io->size - offset;
44 retlen = len;
45 if (io->ops.block_read) {
46 retlen = (*io->ops.block_read)(
47 io, offset, dst, memory_order_seq_cst, len);
48 } else {
49 atomic_thread_fence(memory_order_seq_cst);
50 while ( len && (
51 ((uintptr_t)dest % sizeof(int)) ||
52 ((uintptr_t)ptr % sizeof(int)))) {
53 *(unsigned char *)dest =
54 *(const unsigned char *)ptr;
55 dest++;
56 ptr++;
57 len--;
58 }
59 for (; len >= (int)sizeof(int); dest += sizeof(int),
60 ptr += sizeof(int),
61 len -= sizeof(int))
62 *(unsigned int *)dest = *(const unsigned int *)ptr;
63 for (; len != 0; dest++, ptr++, len--)
64 *(unsigned char *)dest =
65 *(const unsigned char *)ptr;
66 }
67 return retlen;
68 }
69
metal_io_block_write(struct metal_io_region * io,unsigned long offset,const void * restrict src,int len)70 int metal_io_block_write(struct metal_io_region *io, unsigned long offset,
71 const void *restrict src, int len)
72 {
73 unsigned char *ptr = metal_io_virt(io, offset);
74 const unsigned char *source = src;
75 int retlen;
76
77 if (offset > io->size)
78 return -ERANGE;
79 if ((offset + len) > io->size)
80 len = io->size - offset;
81 retlen = len;
82 if (io->ops.block_write) {
83 retlen = (*io->ops.block_write)(
84 io, offset, src, memory_order_seq_cst, len);
85 } else {
86 while ( len && (
87 ((uintptr_t)ptr % sizeof(int)) ||
88 ((uintptr_t)source % sizeof(int)))) {
89 *(unsigned char *)ptr =
90 *(const unsigned char *)source;
91 ptr++;
92 source++;
93 len--;
94 }
95 for (; len >= (int)sizeof(int); ptr += sizeof(int),
96 source += sizeof(int),
97 len -= sizeof(int))
98 *(unsigned int *)ptr = *(const unsigned int *)source;
99 for (; len != 0; ptr++, source++, len--)
100 *(unsigned char *)ptr =
101 *(const unsigned char *)source;
102 atomic_thread_fence(memory_order_seq_cst);
103 }
104 return retlen;
105 }
106
metal_io_block_set(struct metal_io_region * io,unsigned long offset,unsigned char value,int len)107 int metal_io_block_set(struct metal_io_region *io, unsigned long offset,
108 unsigned char value, int len)
109 {
110 unsigned char *ptr = metal_io_virt(io, offset);
111 int retlen = len;
112
113 if (offset > io->size)
114 return -ERANGE;
115 if ((offset + len) > io->size)
116 len = io->size - offset;
117 retlen = len;
118 if (io->ops.block_set) {
119 (*io->ops.block_set)(
120 io, offset, value, memory_order_seq_cst, len);
121 } else {
122 unsigned int cint = value;
123 unsigned int i;
124
125 for (i = 1; i < sizeof(int); i++)
126 cint |= ((unsigned int)value << (8 * i));
127
128 for (; len && ((uintptr_t)ptr % sizeof(int)); ptr++, len--)
129 *(unsigned char *)ptr = (unsigned char) value;
130 for (; len >= (int)sizeof(int); ptr += sizeof(int),
131 len -= sizeof(int))
132 *(unsigned int *)ptr = cint;
133 for (; len != 0; ptr++, len--)
134 *(unsigned char *)ptr = (unsigned char) value;
135 atomic_thread_fence(memory_order_seq_cst);
136 }
137 return retlen;
138 }
139
140