io.c 3.6 KB

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