From 1ad9971d8a87721b8a6a98198e31b3b54d1be7ce Mon Sep 17 00:00:00 2001 From: Alexander Wenzel Date: Fri, 23 Sep 2011 21:12:51 +0200 Subject: Merged version for SHM now works. --- src/shared/dlt_shm.c | 348 +++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 348 insertions(+) create mode 100644 src/shared/dlt_shm.c (limited to 'src/shared/dlt_shm.c') diff --git a/src/shared/dlt_shm.c b/src/shared/dlt_shm.c new file mode 100644 index 0000000..b433648 --- /dev/null +++ b/src/shared/dlt_shm.c @@ -0,0 +1,348 @@ +#include +#include +#include +#include +#include +#include +#include + +#include + +void dlt_shm_print_hex(char *ptr,int size) +{ + int num; + + for (num=0;numshm = NULL; + buf->shmid = 0; + buf->semid = 0; + buf->size = 0; + buf->mem = 0; + + // Create the segment. + if ((buf->shmid = shmget(key, size, IPC_CREAT | 0666)) < 0) { + perror("shmget"); + return -1; /* ERROR */ + } + + // Now we attach the segment to our data space. + if ((buf->shm = shmat(buf->shmid, NULL, 0)) == (char *) -1) { + perror("shmat"); + return -1; /* ERROR */ + } + + // Init semaphore + if( (buf->semid = semget(DLT_SHM_SEM,1,S_IRWXU|S_IRWXG|IPC_CREAT|IPC_EXCL)) == -1 ) { + if( (buf->semid = semget(DLT_SHM_SEM,1,S_IRWXU|S_IRWXG|IPC_EXCL)) == -1 ) { + perror("semget"); + return -1; /* ERROR */ + } + } + if( semctl(buf->semid,0,SETVAL,(int)1) == -1 ) { + perror("semctl"); + return -1; /* ERROR */ + } + + // Init pointers + ((int*)(buf->shm))[0] = 0; // pointer to write memory + ((int*)(buf->shm))[1] = 0; // pointer to read memory + ((int*)(buf->shm))[2] = 0; // number of packets + buf->mem = (char*)(&(((int*)(buf->shm))[3])); + buf->size = size - (buf->mem - buf->shm); + + // clear memory + memset(buf->mem,0,buf->size); + + return 0; /* OK */ +} + +int dlt_shm_init_client(DltShm *buf,int key,int size) { + + // init parameters + buf->shm = NULL; + buf->shmid = 0; + buf->semid = 0; + buf->size = 0; + buf->mem = 0; + + // Create the segment. + if ((buf->shmid = shmget(key, size, 0666)) < 0) { + perror("shmget"); + return -1; /* ERROR */ + } + + // Now we attach the segment to our data space. + if ((buf->shm = shmat(buf->shmid, NULL, 0)) == (char *) -1) { + perror("shmat"); + return -1; /* ERROR */ + } + + // Init semaphore + if( (buf->semid = semget(DLT_SHM_SEM,0,0)) == -1 ) { + perror("semget"); + return -1; /* ERROR */ + } + DLT_SHM_SEM_FREE(buf->semid); + + // Init pointers + buf->mem = (char*)(&(((int*)(buf->shm))[3])); + buf->size = size - (buf->mem - buf->shm); + + return 0; /* OK */ +} + +void dlt_shm_info(DltShm *buf) +{ + + printf("SHM id: %d\n",buf->shmid); + printf("Available size: %d\n",buf->size); + printf("SHM full start address: %lX\n",(unsigned long)buf->shm); + printf("SHM start address: %lX\n",(unsigned long)buf->mem); + +} + +void dlt_shm_status(DltShm *buf) +{ + int write, read, count; + + write = ((int*)(buf->shm))[0]; + read = ((int*)(buf->shm))[1]; + count = ((int*)(buf->shm))[2]; + + printf("Write: %d\n",write); + printf("Read: %d\n",read); + printf("Count: %d\n",count); + +} + +int dlt_shm_push(DltShm *buf,const char *data1, int size1,const char *data2, int size2,const char *data3, int size3) +{ + int write, read, count; + + if(!buf->mem) { + // shm not initialised + printf("SHM not initialised\n"); + return -1; /* ERROR */ + } + + // get semaphore + DLT_SHM_SEM_GET(buf->semid); + + // get current write pointer + write = ((int*)(buf->shm))[0]; + read = ((int*)(buf->shm))[1]; + count = ((int*)(buf->shm))[2]; + + // check space and write pointer + if(read==write && count) { + // shm buffer is full + DLT_SHM_SEM_FREE(buf->semid); + //printf("SHM is totally full\n"); + return -1; // ERROR + } + else if(write >= buf->size) { + if((size1+size2+size3+sizeof(unsigned char)+sizeof(int)) > read) { + DLT_SHM_SEM_FREE(buf->semid); + //printf("SHM is full at start\n"); + return -1; // ERROR + } + write = 0; + } + else if(read > write) { + if((write + size1+size2+size3+sizeof(unsigned char)+sizeof(int)) > read) { + DLT_SHM_SEM_FREE(buf->semid); + //printf("SHM is full at end\n"); + return -1; // ERROR + } + } + else // read <= write + { + if((write + size1+size2+size3 + sizeof(unsigned char)+sizeof(int)) > buf->size) { + // data does not fit at end of buffer + // try write at beginning + if((size1+size2+size3+sizeof(unsigned char)+sizeof(int)) > read) { + DLT_SHM_SEM_FREE(buf->semid); + //printf("SHM is full at start\n"); + return -1; // ERROR + } + // write zero and start buffer at beginning + if((write+sizeof(unsigned char)+sizeof(int)) <= buf->size) { + *((unsigned char*)(buf->mem+write)) = 0; // init write status to unused + *((int*)(buf->mem+write+sizeof(unsigned char))) = 0; // init write size to unused + } + + write = 0; + } + } + + // update global shm pointers + ((int*)(buf->shm))[0] = write+sizeof(unsigned char)+sizeof(int)+size1+size2+size3; // set new write pointer + ((int*)(buf->shm))[2] += 1; // increase counter + + // update buffer pointers + *((unsigned char*)(buf->mem+write)) = 1; // set write status + *((int*)(buf->mem+write+sizeof(unsigned char))) = size1+size2+size3; // set write size + + // write data + if(data1) + memcpy(buf->mem+write+sizeof(unsigned char)+sizeof(int),data1,size1); + if(data2) + memcpy(buf->mem+write+sizeof(unsigned char)+sizeof(int)+size1,data2,size2); + if(data3) + memcpy(buf->mem+write+sizeof(unsigned char)+sizeof(int)+size1+size2,data3,size3); + + // update write status + *((unsigned char*)(buf->mem+write)) = 2; + + // free semaphore + DLT_SHM_SEM_FREE(buf->semid); + + return 0; // OK +} + +int dlt_shm_pull(DltShm *buf,char *data, int max_size) +{ + int write, read, count, size; + unsigned char status; + + if(!buf->mem) { + // shm not initialised + printf("SHM not initialised\n"); + return -1; /* ERROR */ + } + + // get current write pointer + DLT_SHM_SEM_GET(buf->semid); + write = ((int*)(buf->shm))[0]; + read = ((int*)(buf->shm))[1]; + count = ((int*)(buf->shm))[2]; + DLT_SHM_SEM_FREE(buf->semid); + + // check if data is in there + if(count<=0) { + //printf("SHM is empty\n"); + return -1; // ERROR + } + + // check if end of buffer is reached and read status and size + if((read+sizeof(unsigned char)+sizeof(int)) <= buf->size) { + status = *((unsigned char*)(buf->mem+read)); + size = *((int*)(buf->mem+read+sizeof(unsigned char))); + if(status == 0) { + // data fits not end of shm + read = 0; + status = *((unsigned char*)(buf->mem+read)); + size = *((int*)(buf->mem+read+sizeof(unsigned char))); + } + } + else { + read = 0; + status = *((unsigned char*)(buf->mem+read)); + size = *((int*)(buf->mem+read+sizeof(unsigned char))); + } + + // check status + if(status != 2 ) { + printf("Buffer is not fully written\n"); + return -1; // ERROR + } + + // plausibility check of buffer size + if( (read+size) > buf->size) { + printf("Buffers size bigger than shm buffer\n"); + return -1; // ERROR + } + + // check max read size + if(size > max_size) { + printf("Buffer is bigger than max size\n"); + return -1; // ERROR + } + + // copy data + memcpy(data,buf->mem+read+sizeof(unsigned char)+sizeof(int),size); + + // update buffer pointers + ((int*)(buf->shm))[1] = read+sizeof(unsigned char)+sizeof(int)+size; // set new read pointer + ((int*)(buf->shm))[2] -= 1; // decrease counter + + return size; // OK +} + +int dlt_shm_free_server(DltShm *buf) { + + if(!buf->shm) { + printf("Shared memory segment not attached\n"); + return -1; /* ERROR */ + } + + if(shmdt(buf->shm)) { + perror("shmdt"); + return -1; /* ERROR */ + } + + if(shmctl(buf->shmid,IPC_RMID,NULL) == -1) { + perror("shmdt"); + return -1; /* ERROR */ + } + + if(semctl(buf->semid,0,IPC_RMID,(int)0) == -1) { + perror("shmdt"); + return -1; /* ERROR */ + } + + // Reset parameters + buf->shm = NULL; + buf->shmid = 0; + buf->semid = 0; + buf->size = 0; + buf->mem = 0; + + return 0; /* OK */ +} + +int dlt_shm_free_client(DltShm *buf) { + + if(!buf->shm) { + printf("Shared memory segment not attached\n"); + return -1; /* ERROR */ + } + + if(shmdt(buf->shm)) { + perror("shmdt"); + return -1; /* ERROR */ + } + + // Reset parameters + buf->shm = NULL; + buf->shmid = 0; + buf->semid = 0; + buf->size = 0; + buf->mem = 0; + + return 0; /* OK */ +} -- cgit v1.2.1