diff --git a/slsDetectorServers/jungfrauDetectorServer/slsDetectorFunctionList.c b/slsDetectorServers/jungfrauDetectorServer/slsDetectorFunctionList.c index 9b53f906f..8d80d5438 100644 --- a/slsDetectorServers/jungfrauDetectorServer/slsDetectorFunctionList.c +++ b/slsDetectorServers/jungfrauDetectorServer/slsDetectorFunctionList.c @@ -1861,9 +1861,9 @@ int getPrimaryInterface() { return ((bus_r(CONFIG_REG) & CONFIG_INNR_PRIMRY_INTRFCE_MSK) ? 1 : 0); } -void setupHeader(int iRxEntry, enum interfaceType type, uint32_t destip, - uint64_t destmac, uint16_t destport, uint64_t sourcemac, - uint32_t sourceip, uint16_t sourceport) { +int setupHeader(int iRxEntry, enum interfaceType type, uint32_t destip, + uint64_t destmac, uint16_t destport, uint64_t sourcemac, + uint32_t sourceip, uint16_t sourceport) { // start addr uint32_t addr = (type == INNER ? RXR_ENDPOINT_INNER_START_REG @@ -1904,16 +1904,15 @@ void setupHeader(int iRxEntry, enum interfaceType type, uint32_t destip, calcChecksum(udp); - // force FPGA writes to be aligned and in order - uint32_t words[sizeof(udp_header) / sizeof(uint32_t)]; - memcpy(words, udp, sizeof(udp_header)); - for (size_t i = 0; i != sizeof(words) / sizeof(words[0]); ++i) { - bus_w(addr + i * 2, words[i]); + if (bus_w_block(addr, udp, sizeof(udp_header)) == FAIL) { + LOG(logERROR, ("Failed to write UDP header\n")); + return FAIL; } if (iRxEntry < numUdpDestinations) { LOG(logINFO, ("\tIP checksum : 0x%lx\n\n", udp->ip_checksum)); } + return OK; } void calcChecksum(udp_header *udp) { @@ -2030,21 +2029,33 @@ int configureMAC() { #endif if (numInterfaces == 2) { // bottom - setupHeader(iRxEntry, OUTER, dstip, dstmac, dstport, srcmac, srcip, - srcport); + if (setupHeader(iRxEntry, OUTER, dstip, dstmac, dstport, srcmac, + srcip, srcport) == FAIL) { + LOG(logERROR, ("Failed to setup outer header\n")); + return FAIL; + } // top - setupHeader(iRxEntry, INNER, dstip2, dstmac2, dstport2, srcmac2, - srcip2, srcport2); + if (setupHeader(iRxEntry, INNER, dstip2, dstmac2, dstport2, srcmac2, + srcip2, srcport2) == FAIL) { + LOG(logERROR, ("Failed to setup inner header\n")); + return FAIL; + } } // single interface else { // default if (selInterface == 0) { - setupHeader(iRxEntry, OUTER, dstip, dstmac, dstport, srcmac, - srcip, srcport); + if (setupHeader(iRxEntry, OUTER, dstip, dstmac, dstport, srcmac, + srcip, srcport) == FAIL) { + LOG(logERROR, ("Failed to setup outer header\n")); + return FAIL; + } } else { - setupHeader(iRxEntry, INNER, dstip, dstmac, dstport, srcmac, - srcip, srcport2); + if (setupHeader(iRxEntry, INNER, dstip, dstmac, dstport, srcmac, + srcip, srcport2) == FAIL) { + LOG(logERROR, ("Failed to setup inner header\n")); + return FAIL; + } } } } diff --git a/slsDetectorServers/jungfrauDetectorServer/slsDetectorFunctionList.h b/slsDetectorServers/jungfrauDetectorServer/slsDetectorFunctionList.h index e65ed3d37..5170dfeca 100644 --- a/slsDetectorServers/jungfrauDetectorServer/slsDetectorFunctionList.h +++ b/slsDetectorServers/jungfrauDetectorServer/slsDetectorFunctionList.h @@ -167,9 +167,9 @@ int getFirstUDPDestination(); void setFirstUDPDestination(int value); void selectPrimaryInterface(int val); int getPrimaryInterface(); -void setupHeader(int iRxEntry, enum interfaceType type, uint32_t destip, - uint64_t destmac, uint16_t destport, uint64_t sourcemac, - uint32_t sourceip, uint16_t sourceport); +int setupHeader(int iRxEntry, enum interfaceType type, uint32_t destip, + uint64_t destmac, uint16_t destport, uint64_t sourcemac, + uint32_t sourceip, uint16_t sourceport); void calcChecksum(udp_header *udp); int configureMAC(); diff --git a/slsDetectorServers/slsDetectorServer/include/blackfin.h b/slsDetectorServers/slsDetectorServer/include/blackfin.h index d6a14f134..698a19e16 100644 --- a/slsDetectorServers/slsDetectorServer/include/blackfin.h +++ b/slsDetectorServers/slsDetectorServer/include/blackfin.h @@ -50,6 +50,13 @@ void bus_w(u_int32_t offset, u_int32_t data); */ u_int32_t bus_r(u_int32_t offset); +/** + * Force FPGA writes to be aligned and in order + * by writing 32 bit words to the bus + * @returns FAIL if size is not a non-zero multiple of sizeof(uint32_t), else OK + */ +int bus_w_block(u_int32_t offset, const void *src, size_t size); + /** * Read from a 64 bit register * @param aLSB LSB offset address diff --git a/slsDetectorServers/slsDetectorServer/src/blackfin.c b/slsDetectorServers/slsDetectorServer/src/blackfin.c index 868708ab0..64ee3ff23 100644 --- a/slsDetectorServers/slsDetectorServer/src/blackfin.c +++ b/slsDetectorServers/slsDetectorServer/src/blackfin.c @@ -9,6 +9,7 @@ #include // open #include // fopen +#include //memcpy #include // mmap #include // usleep @@ -46,6 +47,22 @@ u_int32_t bus_r(u_int32_t offset) { return *ptr1; } +int bus_w_block(u_int32_t offset, const void *src, size_t size) { + uint32_t step = 1 << MEM_MAP_SHIFT; + if (size == 0 || size % sizeof(uint32_t) != 0) { + LOG(logERROR, ("Could not write block: size %zu is not a non-zero " + "multiple of %zu\n", + size, sizeof(uint32_t))); + return FAIL; + } + uint32_t words[size / sizeof(uint32_t)]; + memcpy(words, src, size); + for (size_t i = 0; i != sizeof(words) / sizeof(words[0]); ++i) { + bus_w(offset + i * step, words[i]); + } + return OK; +} + int64_t get64BitReg(int aLSB, int aMSB) { int64_t v64; u_int32_t vLSB, vMSB; diff --git a/slsDetectorServers/slsDetectorServer/src/common.c b/slsDetectorServers/slsDetectorServer/src/common.c index cd7adab63..7d0abef79 100644 --- a/slsDetectorServers/slsDetectorServer/src/common.c +++ b/slsDetectorServers/slsDetectorServer/src/common.c @@ -817,4 +817,4 @@ int deleteItem(char *mess, int isFile, const char *absPath, char *errorPrefix) { absPath, errorPrefix)); return OK; -} \ No newline at end of file +}