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