aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--hal_io_fmc.c76
1 files changed, 39 insertions, 37 deletions
diff --git a/hal_io_fmc.c b/hal_io_fmc.c
index 8a32921..c8d92ec 100644
--- a/hal_io_fmc.c
+++ b/hal_io_fmc.c
@@ -53,32 +53,17 @@
#define HAL_IO_FMC_DEBUG 0
#endif
-static int debug = 0;
static int inited = 0;
-static inline hal_error_t init(void)
-{
- if (!inited) {
- fmc_init();
- inited = 1;
- }
- return HAL_OK;
-}
+#if HAL_IO_FMC_DEBUG
-/* Translate cryptech register number to FMC address.
- */
-static inline hal_addr_t fmc_offset(hal_addr_t offset)
-{
- return offset << 2;
-}
+static int debug = 0;
void hal_io_set_debug(int onoff)
{
debug = onoff;
}
-#if HAL_IO_FMC_DEBUG
-
static inline void dump(const char *label, const hal_addr_t offset, const uint8_t * const buf, const size_t len)
{
if (debug) {
@@ -95,54 +80,71 @@ static inline void dump(const char *label, const hal_addr_t offset, const uint8_
#endif
-hal_error_t hal_io_write(const hal_core_t *core, hal_addr_t offset, const uint8_t *buf, size_t len)
+/*
+ * Translate cryptech register number to FMC address.
+ */
+static inline hal_addr_t fmc_offset(hal_addr_t offset)
{
- hal_error_t err;
+ return offset << 2;
+}
+
+/*
+ * Minimal version of memcpy, where we know the addresses and length are
+ * word-aligned.
+ */
+static inline void fmc_fpga_memcpy(uint32_t *dst, uint32_t *src, size_t nword)
+{
+ while (nword >= 4) {
+ *dst++ = *src++;
+ *dst++ = *src++;
+ *dst++ = *src++;
+ *dst++ = *src++;
+ nword -= 4;
+ }
+ while (nword > 0) {
+ *dst++ = *src++;
+ nword--;
+ }
+}
+hal_error_t hal_io_write(const hal_core_t *core, hal_addr_t offset, const uint8_t *buf, size_t len)
+{
if (core == NULL)
return HAL_ERROR_CORE_NOT_FOUND;
if (len % 4 != 0)
return HAL_ERROR_IO_BAD_COUNT;
- if ((err = init()) != HAL_OK)
- return err;
+ if (!inited) {
+ fmc_init();
+ inited = 1;
+ }
dump("write ", offset + hal_core_base(core), buf, len);
offset = fmc_offset(offset + hal_core_base(core));
- for (; len > 0; offset += 4, buf += 4, len -= 4) {
- uint32_t val;
- val = htonl(*(uint32_t *)buf);
- fmc_write_32(offset, val);
- }
+ fmc_fpga_memcpy((uint32_t *)fmc_fpga_addr(offset), (uint32_t *)buf, len/4);
return HAL_OK;
}
hal_error_t hal_io_read(const hal_core_t *core, hal_addr_t offset, uint8_t *buf, size_t len)
{
- uint8_t *rbuf = buf;
- int rlen = len;
- hal_error_t err;
-
if (core == NULL)
return HAL_ERROR_CORE_NOT_FOUND;
if (len % 4 != 0)
return HAL_ERROR_IO_BAD_COUNT;
- if ((err = init()) != HAL_OK)
- return err;
+ if (!inited) {
+ fmc_init();
+ inited = 1;
+ }
dump("read ", offset + hal_core_base(core), buf, len);
offset = fmc_offset(offset + hal_core_base(core));
- for (; rlen > 0; offset += 4, rbuf += 4, rlen -= 4) {
- uint32_t val;
- fmc_read_32(offset, &val);
- *(uint32_t *)rbuf = ntohl(val);
- }
+ fmc_fpga_memcpy((uint32_t *)buf, (uint32_t *)fmc_fpga_addr(offset), len/4);
return HAL_OK;
}