From 020237160c7db98e8156e93b1107d546c499984d Mon Sep 17 00:00:00 2001 From: hadaq Date: Wed, 19 Aug 2009 17:41:03 +0000 Subject: [PATCH] Fixed semaphore bug, removed trb_semlock . --- libtrbnet/trbcmd.c | 228 ++++++++++++++++++++++----------------------- libtrbnet/trbnet.c | 188 +++++++++++++++---------------------- libtrbnet/trbnet.h | 1 - 3 files changed, 191 insertions(+), 226 deletions(-) diff --git a/libtrbnet/trbcmd.c b/libtrbnet/trbcmd.c index 2d66cb6..82bc2cb 100644 --- a/libtrbnet/trbcmd.c +++ b/libtrbnet/trbcmd.c @@ -30,8 +30,8 @@ void usage(const char *progName) printf(" -f execute commands given in script-file\n"); printf(" -n repeat COMMAND number times, -1 = endless loop\n"); printf(" -d turn on Debugging Information\n"); - printf(" level 1: TRB_Package debugging\n"); - printf(" level 2: +FIFO debugging\n"); + printf(" level 1: TRB_Package debugging\n"); + printf(" level 2: +FIFO debugging\n"); printf(" -D FIFO DMA-Mode\n"); printf(" -l lazy-mode: skip most consistency-checks of packages\n"); printf(" -H hex-mode: all following arguments will be interpreted " @@ -47,7 +47,7 @@ void usage(const char *progName) printf(" i -> read unique ID\n"); printf(" s -> set trb-address\n"); printf(" T -> send trigger\n"); - printf(" TR -> send trigger to " + printf(" TR -> send trigger to " "RICH only\n"); printf(" I -> read IPU data\n"); printf(" f -> flush FIFO of " @@ -73,14 +73,13 @@ int main(int argc, char ** argv) size_t cmdLineLen = 0; unsigned int cmdLen = 0; uint16_t trb_address = 0; - uint16_t reg_address = 0; + uint16_t reg_address = 0; int loop = 1; int loopCtr = 0; int opt; - + trb_debug = 0; trb_lazy = 0; - trb_semlock = 1; /* Parse Arguments */ while ((opt = getopt(argc, argv, "+hf:n:d:DlHV")) != -1) { @@ -117,7 +116,7 @@ int main(int argc, char ** argv) break; } } - + /* Open scriptFile if requested */ if (strlen(scriptFileName) > 0) { if (strncmp(scriptFileName, "-", 256) == 0) { @@ -132,23 +131,23 @@ int main(int argc, char ** argv) } } } - + /* Open ports */ - init_ports(); + init_ports(); /* Start repeat-loop */ while ((loop == -1) || (loopCtr++ < loop)) { unsigned int lineCtr = 0; ssize_t scriptStatus = 0; - + /* Start script-file-loop */ - while (scriptStatus != -1) { - if (scriptFile == NULL) { + while (scriptStatus != -1) { + if (scriptFile == NULL) { /* Get command from function-call */ unsigned int i; cmdLen = argc - optind; for (i = 0; (i < cmdLen) && (i < CMD_MAX_NUM); i++) { - strncpy(cmd[i], argv[optind + i], CMD_SIZE); + strncpy(cmd[i], argv[optind + i], CMD_SIZE); } scriptStatus = -1; } else { @@ -156,13 +155,13 @@ int main(int argc, char ** argv) char *c = NULL; unsigned int i; - + lineCtr++; /* Initialize */ for (i = 0; i < CMD_MAX_NUM; i++) { cmd[i][0] = '\0'; } - + if ((scriptStatus = getline(&cmdLine, &cmdLineLen, scriptFile)) == -1) { if (feof(scriptFile) != 0) { @@ -175,7 +174,7 @@ int main(int argc, char ** argv) exit(EXIT_FAILURE); } } - + /* Remove newline and comments */ if ((c = strchr(cmdLine, '\n')) != NULL) { *c = '\0'; @@ -185,10 +184,10 @@ int main(int argc, char ** argv) } /* Split up cmdLine */ - sscanf(cmdLine, "%s %s %s %s %s %s %s %s %s %s", + sscanf(cmdLine, "%s %s %s %s %s %s %s %s %s %s", cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5], cmd[6], cmd[7], cmd[8], cmd[9]); - + for (i = 0, cmdLen = 0; i < CMD_MAX_NUM; i++, cmdLen++) { if (cmd[i][0] == '\0') break; } @@ -196,17 +195,18 @@ int main(int argc, char ** argv) /* Empty Line */ continue; } + fprintf(stdout, "#Line %d: %s\n", lineCtr, cmdLine); } - + if (strncmp(cmd[0], "w", CMD_SIZE) == 0) { - + /*******************************************/ /* Register Write */ /*******************************************/ - + uint32_t value = 0; - + if (cmdLen != 4) { if (scriptFile != NULL) { fprintf(stderr, "Line #%d: Invalid command\n", lineCtr); @@ -215,16 +215,16 @@ int main(int argc, char ** argv) } exit(EXIT_FAILURE); } - + trb_address = strtoul(cmd[1], NULL, hexMode == 1 ? 16 : 0); reg_address = strtoul(cmd[2], NULL, hexMode == 1 ? 16 : 0); value = strtoul(cmd[3], NULL, hexMode == 1 ? 16 : 0); - + /* DEBUG Info */ if (trb_debug > 0) { - fprintf(stderr, + fprintf(stderr, "Command: WRITE: trb_address: 0x%04x, reg_address: 0x%04x, " - "value: 0x%08x\n", + "value: 0x%08x\n", trb_address, reg_address, value); } @@ -246,7 +246,7 @@ int main(int argc, char ** argv) int status = 0; uint32_t data[256]; int i; - + if (cmdLen != 3) { if (scriptFile != NULL) { fprintf(stderr, "Line #%d: Invalid command\n", lineCtr); @@ -255,18 +255,18 @@ int main(int argc, char ** argv) } exit(EXIT_FAILURE); } - + trb_address = strtoul(cmd[1], NULL, hexMode == 1 ? 16 : 0); reg_address = strtoul(cmd[2], NULL, hexMode == 1 ? 16 : 0); /* DEBUG Info */ if (trb_debug > 0) { - fprintf(stderr, + fprintf(stderr, "Command: READ: trb_address: 0x%04x, " "reg_address: 0x%04x\n", trb_address, reg_address); } - + status = trb_register_read(trb_address, reg_address, data, 256); if (status == -1) { if (scriptFile != NULL) { @@ -277,17 +277,17 @@ int main(int argc, char ** argv) exit(EXIT_FAILURE); } } - + for (i = 0; i < status; i += 2) { fprintf(stdout, "0x%04x 0x%08x\n", data[i], data[i + 1]); } } else if (strncmp(cmd[0], "rm", CMD_SIZE) == 0) { - + /*******************************************/ /* Register Read Memory */ /*******************************************/ - + uint32_t *data = NULL; uint16_t size = 0; uint8_t option = 0; @@ -296,7 +296,7 @@ int main(int argc, char ** argv) const uint32_t* end = NULL; unsigned int len; unsigned int i; - + if (cmdLen != 5) { if (scriptFile != NULL) { fprintf(stderr, "Line #%d: Invalid command\n", lineCtr); @@ -305,15 +305,15 @@ int main(int argc, char ** argv) } exit(EXIT_FAILURE); } - + trb_address = strtoul(cmd[1], NULL, hexMode == 1 ? 16 : 0); reg_address = strtoul(cmd[2], NULL, hexMode == 1 ? 16 : 0); size = strtoul(cmd[3], NULL, hexMode == 1 ? 16 : 0); option = strtoul(cmd[4], NULL, hexMode == 1 ? 16 : 0); - + /* DEBUG Info */ if (trb_debug > 0) { - fprintf(stderr, + fprintf(stderr, "Command: READ_MEM: " "trb_address: 0x%04x, " "reg_address: 0x%04x, " @@ -321,11 +321,11 @@ int main(int argc, char ** argv) "option: %d\n", trb_address, reg_address, size, option); } - + if ((data = malloc(sizeof(uint32_t) * READ_MEM_SIZE)) == NULL) abort(); - - status = - trb_register_read_mem(trb_address, reg_address, option, + + status = + trb_register_read_mem(trb_address, reg_address, option, size, data, READ_MEM_SIZE); if (status == -1) { if (scriptFile != NULL) { @@ -337,29 +337,29 @@ int main(int argc, char ** argv) exit(EXIT_FAILURE); } } - + /* Print data-buffer */ p = data; end = p + status; while (p < end) { len = (*p >> 16) & 0xffff; fprintf(stdout, "0x%04x 0x%04x\n", (*p++) & 0xffff, len); - for (i = 0; (i < len) && (p < end); i++) { - fprintf(stdout, "0x%04x 0x%08x\n", + for (i = 0; (i < len) && (p < end); i++) { + fprintf(stdout, "0x%04x 0x%08x\n", reg_address + i, *p++); } } - + if (data != NULL) free(data); } else if (strncmp(cmd[0], "wm", CMD_SIZE) == 0) { - + /*******************************************/ /* Register Write Memory */ /*******************************************/ - + FILE *file = NULL; uint32_t *data = NULL; - unsigned int dataSize = 64; + unsigned int dataSize = 64; char *line = NULL; size_t len = 0; char *fileName = NULL; @@ -375,12 +375,12 @@ int main(int argc, char ** argv) } exit(EXIT_FAILURE); } - + trb_address = strtoul(cmd[1], NULL, hexMode == 1 ? 16 : 0); reg_address = strtoul(cmd[2], NULL, hexMode == 1 ? 16 : 0); option = strtoul(cmd[3], NULL, hexMode == 1 ? 16 : 0); fileName = cmd[4]; - + /* Open inputFile and read Data into buffer */ if (strncmp(fileName, "-", CMD_SIZE) == 0) { file = stdin; @@ -391,7 +391,7 @@ int main(int argc, char ** argv) exit(EXIT_FAILURE); } } - + if ((data = malloc(sizeof(uint32_t) * dataSize)) == NULL) abort(); while (getline(&line, &len, file) != -1) { if (size >= dataSize) { @@ -403,10 +403,10 @@ int main(int argc, char ** argv) data[size++] = strtoul(line, NULL, hexMode == 1 ? 16 : 0); } if (line != NULL) free(line); - + /* DEBUG Info */ if (trb_debug > 0) { - fprintf(stderr, + fprintf(stderr, "Command: WRITE_MEM: trb_address: 0x%04x, " "reg_address: 0x%04x, " "option: %d, " @@ -414,11 +414,11 @@ int main(int argc, char ** argv) "size: 0x%04x\n", trb_address, reg_address, option, fileName, size); } - - status = trb_register_write_mem(trb_address, reg_address, option, + + status = trb_register_write_mem(trb_address, reg_address, option, data, size); if (data != NULL) free(data); - + if (status == -1) { if (scriptFile != NULL) { fprintf(stderr, "Line #%d: ", lineCtr); @@ -433,11 +433,11 @@ int main(int argc, char ** argv) /*******************************************/ /* ReadUId */ /*******************************************/ - + uint32_t uidBuffer[512]; int status; int i; - + if (cmdLen != 2) { if (scriptFile != NULL) { fprintf(stderr, "Line #%d: Invalid command\n", lineCtr); @@ -446,16 +446,16 @@ int main(int argc, char ** argv) } exit(EXIT_FAILURE); } - + trb_address = strtoul(cmd[1], NULL, hexMode == 1 ? 16 : 0); - + /* DEBUG Info */ if (trb_debug > 0) { - fprintf(stderr, + fprintf(stderr, "Command: READ_UID: trb_address: 0x%04x\n", trb_address); } - + status = trb_read_uid(trb_address, uidBuffer, 128); if (status == -1) { if (scriptFile != NULL) { @@ -466,24 +466,24 @@ int main(int argc, char ** argv) exit(EXIT_FAILURE); } } - + for (i = 0; (i < status) && (i < 128); i += 4) { fprintf(stdout, "0x%04x 0x%08x%08x 0x%02x\n", uidBuffer[i + 3], - uidBuffer[i], - uidBuffer[i + 1], + uidBuffer[i], + uidBuffer[i + 1], uidBuffer[i + 2]); } } else if (strncmp(cmd[0], "s", CMD_SIZE) == 0) { - + /*******************************************/ /* SetAddress */ /*******************************************/ - + uint64_t uid = 0; uint8_t endpoint = 0; uint16_t trb_address = 0; - + if (cmdLen != 4) { if (scriptFile != NULL) { fprintf(stderr, "Line #%d: Invalid command\n", lineCtr); @@ -492,21 +492,21 @@ int main(int argc, char ** argv) } exit(EXIT_FAILURE); } - + uid = strtoull(cmd[1], NULL, hexMode == 1 ? 16 : 0); endpoint = strtoul(cmd[2], NULL, hexMode == 1 ? 16 : 0); trb_address = strtoul(cmd[3], NULL, hexMode == 1 ? 16 : 0); - + /* DEBUG Info */ if (trb_debug > 0) { - fprintf(stderr, + fprintf(stderr, "Command: SET_ADDRESS: " "uid: 0x%016llx, " "endpoint: 0x%02x, " "trb_address: 0x%04x\n", uid, endpoint, trb_address); } - + if (trb_set_address(uid, endpoint, trb_address) == -1) { if (scriptFile != NULL) { fprintf(stderr, "Line #%d: ", lineCtr); @@ -517,16 +517,16 @@ int main(int argc, char ** argv) } } } else if (strncmp(cmd[0], "T", CMD_SIZE) == 0) { - + /*******************************************/ /* Send Trigger */ /*******************************************/ - + uint8_t type; uint8_t random = 0; uint8_t info = 0; uint16_t number = 0; - + if (cmdLen != 5) { if (scriptFile != NULL) { fprintf(stderr, "Line #%d: Invalid command\n", lineCtr); @@ -535,15 +535,15 @@ int main(int argc, char ** argv) } exit(EXIT_FAILURE); } - + type = strtoul(cmd[2], NULL, hexMode == 1 ? 16 : 0) & 0x0f; random = strtoul(cmd[3], NULL, hexMode == 1 ? 16 : 0); info = strtoul(cmd[4], NULL, hexMode == 1 ? 16 : 0); number = strtoul(cmd[5], NULL, hexMode == 1 ? 16 : 0); - + /* DEBUG Info */ if (trb_debug > 0) { - fprintf(stderr, + fprintf(stderr, "Command: SEND_TRIGGER: " "type: 0x%01x, " "random: 0x%02x, " @@ -551,7 +551,7 @@ int main(int argc, char ** argv) "number: 0x%04x\n", type, random, info, number); } - + if (trb_send_trigger(type, info, random, number) == -1) { if (scriptFile != NULL) { fprintf(stderr, "Line #%d: ", lineCtr); @@ -560,19 +560,19 @@ int main(int argc, char ** argv) if (trb_errno != TRB_ENDPOINT_NOT_REACHED) { exit(EXIT_FAILURE); } - } + } } else if (strncmp(cmd[0], "TR", CMD_SIZE) == 0) { - + /*********************************************/ /* Send Fake trigger function to RICH Subnet */ /*********************************************/ - + uint8_t input = 0; uint8_t type = 0; uint8_t random = 0; uint8_t info = 0; uint16_t number = 0; - + if (cmdLen != 6) { if (scriptFile != NULL) { fprintf(stderr, "Line #%d: Invalid command\n", lineCtr); @@ -581,16 +581,16 @@ int main(int argc, char ** argv) } exit(EXIT_FAILURE); } - + input = strtoul(cmd[1], NULL, hexMode == 1 ? 16 : 0) & 0x03; type = strtoul(cmd[2], NULL, hexMode == 1 ? 16 : 0) & 0x0f; random = strtoul(cmd[3], NULL, hexMode == 1 ? 16 : 0); info = strtoul(cmd[4], NULL, hexMode == 1 ? 16 : 0); number = strtoul(cmd[5], NULL, hexMode == 1 ? 16 : 0); - + /* DEBUG Info */ if (trb_debug > 0) { - fprintf(stderr, + fprintf(stderr, "Command: SEND_TRIGGER: " "input: 0x%01x, " "type: 0x%01x, " @@ -599,7 +599,7 @@ int main(int argc, char ** argv) "number: 0x%04x\n", input, type, random, info, number); } - + if (trb_send_trigger_rich(input, type, info, random, number) == -1) { if (scriptFile != NULL) { fprintf(stderr, "Line #%d: ", lineCtr); @@ -610,11 +610,11 @@ int main(int argc, char ** argv) } } } else if (strncmp(cmd[0], "I", CMD_SIZE) == 0) { - + /*******************************************/ /* IPU channel readout */ /*******************************************/ - + uint32_t buffer[4096]; uint8_t type = 0; uint8_t random = 0; @@ -622,7 +622,7 @@ int main(int argc, char ** argv) uint16_t number = 0; int status = 0; int i; - + if (cmdLen != 5) { if (scriptFile != NULL) { fprintf(stderr, "Line #%d: Invalid command\n", lineCtr); @@ -631,15 +631,15 @@ int main(int argc, char ** argv) } exit(EXIT_FAILURE); } - + type = strtoul(cmd[1], NULL, hexMode == 1 ? 16 : 0) & 0x0f; random = strtoul(cmd[2], NULL, hexMode == 1 ? 16 : 0); info = strtoul(cmd[3], NULL, hexMode == 1 ? 16 : 0); number = strtoul(cmd[4], NULL, hexMode == 1 ? 16 : 0); - + /* DEBUG Info */ if (trb_debug > 0) { - fprintf(stderr, + fprintf(stderr, "Command: READ_IPU_DATA: " "type: 0x%01x, " "random: 0x%02x, " @@ -657,19 +657,19 @@ int main(int argc, char ** argv) exit(EXIT_FAILURE); } } - + for (i = 0; i < status; i++) { fprintf(stdout, "0x%08x\n", buffer[i]); } } else if (strncmp(cmd[0], "f", CMD_SIZE) == 0) { - + /*******************************************/ /* Flush FIFO Channel */ /*******************************************/ - + int status; uint8_t channel = 0; - + if (cmdLen != 2) { if (scriptFile != NULL) { fprintf(stderr, "Line #%d: Invalid command\n", lineCtr); @@ -678,12 +678,12 @@ int main(int argc, char ** argv) } exit(EXIT_FAILURE); } - + channel = strtoul(cmd[1], NULL, hexMode == 1 ? 16 : 0); if (trb_debug > 0) { fprintf(stderr, "Command: FIFO_FLUSH_CHANNEL #%d\n", channel); } - + status = trb_fifo_flush(channel); if (status == -1) { if (scriptFile != NULL) { @@ -695,14 +695,14 @@ int main(int argc, char ** argv) } } } else if (strncmp(cmd[0], "R", CMD_SIZE) == 0) { - + /*******************************************/ /* Read FIFO Register */ /*******************************************/ - + uint32_t value = 0; uint16_t reg_address = 0; - + if (cmdLen != 2) { if (scriptFile != NULL) { fprintf(stderr, "Line #%d: Invalid command\n", lineCtr); @@ -715,24 +715,24 @@ int main(int argc, char ** argv) reg_address = strtoul(cmd[1], NULL, hexMode == 1 ? 16 : 0); if (trb_debug > 0) { - fprintf(stderr, + fprintf(stderr, "Command: READ_FIFO_REGISTER:" "reg_address: 0x%04x\n", reg_address); } - + read32_from_FPGA(reg_address, &value); fprintf(stdout, "0x%04x 0x%08x\n", reg_address, value); - + } else if (strncmp(cmd[0], "W", CMD_SIZE) == 0) { - + /*******************************************/ /* Write FIFO Register */ /*******************************************/ - + uint32_t value = 0; uint16_t reg_address = 0; - + if (cmdLen != 3) { if (scriptFile != NULL) { fprintf(stderr, "Line #%d: Invalid command\n", lineCtr); @@ -741,18 +741,18 @@ int main(int argc, char ** argv) } exit(EXIT_FAILURE); } - + reg_address = strtoul(cmd[1], NULL, hexMode == 1 ? 16 : 0); value = strtoul(cmd[2], NULL, hexMode == 1 ? 16 : 0); if (trb_debug > 0) { - fprintf(stderr, + fprintf(stderr, "Command: READ_FIFO_REGISTER:" "reg_address: 0x%04x, " "value: 0x%08x\n", reg_address, value); } - + write32_to_FPGA(reg_address, value); } else { if (scriptFile != NULL) { @@ -763,14 +763,14 @@ int main(int argc, char ** argv) exit(EXIT_FAILURE); } } /* End script-file-loop */ - + } /* End repeat-loop */ - + /* Cleanup */ if (scriptFile != NULL) { fclose(scriptFile); } if (cmdLine != 0) free(cmdLine); - + exit(EXIT_SUCCESS); } diff --git a/libtrbnet/trbnet.c b/libtrbnet/trbnet.c index e608a38..8fa89ce 100644 --- a/libtrbnet/trbnet.c +++ b/libtrbnet/trbnet.c @@ -2,8 +2,11 @@ #include #include #include +#include +#include #include #include +#include #include #include @@ -129,9 +132,8 @@ /* ---------------------------------------------------------------------- */ /* Used for blocking Signals SIGINT and SIGTERM */ -static sigset_t blockSet; +static sigset_t blockSet; static sigset_t blockSetOld; -static int signalsBlocked = 0; /* Semaphore handling */ static int semid = -1; @@ -140,7 +142,6 @@ static const key_t sem_key = 0x545242; unsigned int trb_debug = 0; unsigned int trb_lazy = 0; unsigned int trb_dma = 0; -unsigned int trb_semlock = 0; /* Declaration of a TRB-Package */ @@ -420,7 +421,7 @@ static int trb_fifo_read(uint8_t channel, /* Read FIFO-Buffer, copy to User-Buffer */ while ((*tmp & MASK_FIFO_VALID) != 0) { fifoDebugCtr++; - + if (((*tmp & MASK_FIFO_TYPE) >> SHIFT_FIFO_TYPE) == FIFO_TYPE_IS_HEADER) { /* TRBNet HEADER */ if ((counter % 5) == 0) { @@ -431,7 +432,7 @@ static int trb_fifo_read(uint8_t channel, } packageCtr++; counter = 0; - + /* DEBUG INFO */ if (trb_debug > 1) { fprintf(stderr, "FIFO_%03d: 0x%08x\n", @@ -439,28 +440,28 @@ static int trb_fifo_read(uint8_t channel, } } else { /* Error: invalid buffer content, flush FIFO-BUFFER and exit */ - + /* DEBUG INFO */ if (trb_debug > 1) { fprintf(stderr, "FIFO_%03d: 0x%08x\n", fifoDebugCtr, *tmp); } - + fifo_flush(channel); trb_errno = TRB_FIFO_INCOMPLETE_PACKAGE; return -1; } } else { /* TRBNet Data Word */ - + /* DEBUG INFO */ if (trb_debug > 1) { fprintf(stderr, "FIFO_%03d: 0x%08x\n", fifoDebugCtr, *tmp); } - + if ((trb_lazy == 0) && - (((*tmp & MASK_FIFO_TYPE) >> SHIFT_FIFO_TYPE) != + (((*tmp & MASK_FIFO_TYPE) >> SHIFT_FIFO_TYPE) != (counter - 1) % 2)) { /* Error: invalid sequence (not 0, 1, .), flush FIFO-BUFFER and exit */ fifo_flush(channel); @@ -821,64 +822,43 @@ static int trb_fifo_read(uint8_t channel, return dataCtr; } -static void blockSignals() -{ - signalsBlocked++; - if (signalsBlocked > 1) return; - sigprocmask(SIG_BLOCK, &blockSet, &blockSetOld); -} - -static void unblockSignals() -{ - signalsBlocked--; - if (signalsBlocked > 0) return; - sigprocmask(SIG_SETMASK, &blockSetOld, NULL); -} - static int lockPorts() { - struct sembuf sops[2] = { - { - 0, /* sem_num: We only use one track */ - 0, /* sem_op: wait for semaphore flag to become zero */ - SEM_UNDO /* sem_flg: remove sem if process gets killed */ - }, - { - 0, /* sem_num: We only use one track */ - 1, /* sem_op: increment semaphore, i.e. lock it */ - SEM_UNDO | IPC_NOWAIT /* sem_flg: remove sem if process gets killed */ - } + struct sembuf sops = { + 0, /* sem_num: We only use one track */ + -1, /* sem_op: decrement semaphore by 1, i.e. lock it */ + SEM_UNDO /* sem_flg: remove lock if process gets killed */ }; - - if (trb_semlock == 0) return 0; - - if (semop(semid, sops, 2) == -1) { - perror("TRB_SEMAPHORE lock"); + + /* Wait for semaphore and lock it */ + if (semop(semid, &sops, 1) == -1) { trb_errno = TRB_SEMAPHORE; return -1; } + /* Block Signals */ + sigprocmask(SIG_BLOCK, &blockSet, &blockSetOld); + return 0; } static int unlockPorts() { - struct sembuf sops[1] = { - { - 0, /* sem_num: We only use one track */ - -1, /* sem_op: decrement semaphore, i.e. unlock it */ - IPC_NOWAIT /* */ - } + struct sembuf sops = { + 0, /* sem_num: We only use one track */ + 1, /* sem_op: decrement semaphore, i.e. unlock it */ + SEM_UNDO /* */ }; - - if (trb_semlock == 0) return 0; - - if (semop(semid, sops, 1) == -1) { - perror("TRB_SEMAPHORE unlock"); + + /* Unblock Signals */ + sigprocmask(SIG_SETMASK, &blockSetOld, NULL); + + /* Release semaphore */ + if (semop(semid, &sops, 1) == -1) { trb_errno = TRB_SEMAPHORE; return -1; } - + return 0; } @@ -898,14 +878,31 @@ int init_ports() return -1; } - /* Get / Create semaphore */ - if (trb_semlock > 0) { - if ((semid = semget(sem_key, 1, IPC_CREAT | 0666)) == -1) { + /* Get / Create semaphore */ + if ((semid = semget(sem_key, 1, + IPC_CREAT | IPC_EXCL | + S_IRUSR | S_IWUSR | + S_IRGRP | S_IWGRP | + S_IROTH | S_IWOTH)) == -1) { + if (errno == EEXIST) { + /* Semaphore already exist, so just open it */ + if ((semid = semget(sem_key, 1, 0)) == -1) { + trb_errno = TRB_SEMAPHORE; + return -1; + } + } else { + /* Fatal error, see errno */ + trb_errno = TRB_SEMAPHORE; + return -1; + } + } else { + /* Initialize newly created semaphore, i.e set value to 1 */ + if (semctl(semid, 0, SETVAL, 1) == -1) { trb_errno = TRB_SEMAPHORE; return -1; } } - + return 0; } @@ -927,15 +924,11 @@ int trb_fifo_flush(uint8_t channel) if (trb_debug > 1) { fprintf(stderr, "Flushing FIFO of channel# %d\n", channel); } - + if (lockPorts() == -1) return -1; - blockSignals(); - fifo_flush(channel); - - unblockSignals(); - + if (unlockPorts() == -1) return -1; return 0; @@ -954,6 +947,7 @@ int trb_register_read(uint16_t trb_address, /* Init transfer */ if (trb_init_transfer(3) == -1) { + unlockPorts(); return -1; } @@ -961,9 +955,7 @@ int trb_register_read(uint16_t trb_address, if (trb_debug > 0) { fprintf(stderr, "Init_Transfer done.\n"); } - - blockSignals(); - + /* Build up package and start transfer */ write32_to_FPGA(CHANNEL_3_TARGET_ADDRESS, trb_address); write32_to_FPGA(CHANNEL_3_SENDER_ERROR, 0x00000000); @@ -980,10 +972,8 @@ int trb_register_read(uint16_t trb_address, status = trb_fifo_read(3, FIFO_MODE_REG_READ, data, dsize); - unblockSignals(); - if (unlockPorts() == -1) return -1; - + if ((status > 0) && (status % 2 != 0)) { trb_errno = TRB_INVALID_PKG_NUMBER; return -1; @@ -1018,6 +1008,7 @@ int trb_register_read_mem(uint16_t trb_address, /* Init transfer */ if (trb_init_transfer(3) == -1) { + unlockPorts(); return -1; } @@ -1025,9 +1016,7 @@ int trb_register_read_mem(uint16_t trb_address, if (trb_debug > 0) { fprintf(stderr, "Init_Tranfer done.\n"); } - - blockSignals(); - + /* Build up package and start transfer */ write32_to_FPGA(CHANNEL_3_TARGET_ADDRESS, trb_address); write32_to_FPGA(CHANNEL_3_SENDER_ERROR, 0x00000000); @@ -1044,8 +1033,6 @@ int trb_register_read_mem(uint16_t trb_address, status = trb_fifo_read(3, FIFO_MODE_REG_READ_MEM, data, dsize); - unblockSignals(); - if (unlockPorts() == -1) return -1; if (status == -1) return status; @@ -1071,13 +1058,14 @@ int trb_register_write(uint16_t trb_address, uint32_t value) { int status; - + trb_errno = TRB_NONE; if (lockPorts() == -1) return -1; - + /* Init transfer */ if (trb_init_transfer(3) == -1) { + unlockPorts(); return -1; } @@ -1086,8 +1074,6 @@ int trb_register_write(uint16_t trb_address, fprintf(stderr, "Init_Transfer done.\n"); } - blockSignals(); - /* Build up package */ write32_to_FPGA(CHANNEL_3_TARGET_ADDRESS, trb_address); write32_to_FPGA(CHANNEL_3_SENDER_ERROR, 0x00000000); @@ -1104,8 +1090,6 @@ int trb_register_write(uint16_t trb_address, status = trb_fifo_read(3, FIFO_MODE_REG_WRITE, NULL, 0); - unblockSignals(); - if (unlockPorts() == -1) return -1; return status; @@ -1135,6 +1119,7 @@ int trb_register_write_mem(uint16_t trb_address, /* Init transfer */ if (trb_init_transfer(3) == -1) { + unlockPorts(); return -1; } @@ -1143,8 +1128,6 @@ int trb_register_write_mem(uint16_t trb_address, fprintf(stderr, "Init_Transfer done.\n"); } - blockSignals(); - /* Build up package */ write32_to_FPGA(CHANNEL_3_TARGET_ADDRESS, trb_address); write32_to_FPGA(CHANNEL_3_SENDER_ERROR, 0x00000000); @@ -1167,8 +1150,6 @@ int trb_register_write_mem(uint16_t trb_address, status = trb_fifo_read(3, FIFO_MODE_REG_WRITE, NULL, 0); - unblockSignals(); - if (unlockPorts() == -1) return -1; return status; @@ -1186,6 +1167,7 @@ int trb_read_uid(uint16_t trb_address, /* Init transfer */ if (trb_init_transfer(3) == -1) { + unlockPorts(); return -1; } @@ -1194,8 +1176,6 @@ int trb_read_uid(uint16_t trb_address, fprintf(stderr, "Init_Transfer done.\n"); } - blockSignals(); - /* Build up package and start transfer */ write32_to_FPGA(CHANNEL_3_TARGET_ADDRESS, trb_address); write32_to_FPGA(CHANNEL_3_SENDER_ERROR, 0x00000000); @@ -1212,8 +1192,6 @@ int trb_read_uid(uint16_t trb_address, status = trb_fifo_read(3, FIFO_MODE_UID, (uint32_t*)data, dsize); - unblockSignals(); - if (unlockPorts() == -1) return -1; if ((status > 0) && (status % 4 != 0)) { @@ -1240,9 +1218,10 @@ int trb_set_address(uint64_t uid, } if (lockPorts() == -1) return -1; - + /* Init transfer */ if (trb_init_transfer(3) == -1) { + unlockPorts(); return -1; } @@ -1251,8 +1230,6 @@ int trb_set_address(uint64_t uid, fprintf(stderr, "Init_Transfer done.\n"); } - blockSignals(); - /* Build up package and start transfer */ write32_to_FPGA(CHANNEL_3_TARGET_ADDRESS, 0xffff); /* always broadcast */ write32_to_FPGA(CHANNEL_3_SENDER_ERROR, 0x00000000); @@ -1272,9 +1249,7 @@ int trb_set_address(uint64_t uid, } status = trb_fifo_read(3, FIFO_MODE_SET_ADDRESS, NULL, 0); - - unblockSignals(); - + if (unlockPorts() == -1) return -1; if (status == -1) return -1; @@ -1300,9 +1275,10 @@ int trb_ipu_data_read(uint8_t type, if (data == NULL) return -1; if (lockPorts() == -1) return -1; - + /* Init transfer IPU Channel */ if (trb_init_transfer(1) == -1) { + unlockPorts(); return -1; } @@ -1311,8 +1287,6 @@ int trb_ipu_data_read(uint8_t type, fprintf(stderr, "Init_Transfer done.\n"); } - blockSignals(); - /* Prepare IPU channel */ write32_to_FPGA(CHANNEL_1_SENDER_ERROR, (((uint32_t)trg_info << 24) | ((uint32_t)trg_random << 16) | @@ -1328,8 +1302,6 @@ int trb_ipu_data_read(uint8_t type, status = trb_fifo_read(1, FIFO_MODE_IPU_DATA, data, dsize); - unblockSignals(); - if (unlockPorts() == -1) return -1; return status; @@ -1349,6 +1321,7 @@ int trb_send_trigger(uint8_t type, /* Init transfer trigger */ if (trb_init_transfer(0) == -1) { + unlockPorts(); return -1; } @@ -1357,8 +1330,6 @@ int trb_send_trigger(uint8_t type, fprintf(stderr, "Init_Transfer done.\n"); } - blockSignals(); - /* Prepare trigger channel */ write32_to_FPGA(CHANNEL_0_SENDER_ERROR, (((uint32_t)trg_info << 24) | ((uint32_t)trg_random << 16) | @@ -1375,11 +1346,9 @@ int trb_send_trigger(uint8_t type, /* Check for replay packets (trigger) */ status = trb_fifo_read(0, FIFO_MODE_NONE, NULL, 0); - - unblockSignals(); - + if (unlockPorts() == -1) return -1; - + if (status == -1) return -1; return 0; @@ -1399,11 +1368,13 @@ int trb_send_trigger_rich(uint8_t trg_input, /* Init transfer slowcontrol */ if (trb_init_transfer(3) == -1) { + unlockPorts(); return -1; } /* Init transfer trigger */ if (trb_init_transfer(0) == -1) { + unlockPorts(); return -1; } @@ -1412,8 +1383,6 @@ int trb_send_trigger_rich(uint8_t trg_input, fprintf(stderr, "Init_Transfer done.\n"); } - blockSignals(); - /* Prepare trigger channel */ write32_to_FPGA(CHANNEL_0_SENDER_ERROR, (((uint32_t)trg_info << 24) | ((uint32_t)trg_random << 16) | @@ -1441,16 +1410,13 @@ int trb_send_trigger_rich(uint8_t trg_input, status = trb_fifo_read(3, FIFO_MODE_NONE, NULL, 0); if (status == -1) { fifo_flush(0); - unblockSignals(); - if (unlockPorts() == -1) return -1; + unlockPorts(); return -1; } /* Check for replay packets (trigger) */ status = trb_fifo_read(0, FIFO_MODE_NONE, NULL, 0); - - unblockSignals(); - + if (unlockPorts() == -1) return -1; if (status == -1) return -1; diff --git a/libtrbnet/trbnet.h b/libtrbnet/trbnet.h index effdf63..7c27a47 100644 --- a/libtrbnet/trbnet.h +++ b/libtrbnet/trbnet.h @@ -6,7 +6,6 @@ extern unsigned int trb_debug; extern unsigned int trb_lazy; extern unsigned int trb_dma; -extern unsigned int trb_semlock; /* ---------------------------------------------------------------------- */ -- 2.43.0