summaryrefslogtreecommitdiff
path: root/src/shared
diff options
context:
space:
mode:
Diffstat (limited to 'src/shared')
-rw-r--r--src/shared/dlt_shm.c348
1 files changed, 348 insertions, 0 deletions
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 <sys/types.h>
+#include <sys/ipc.h>
+#include <sys/shm.h>
+#include <sys/sem.h>
+#include <sys/stat.h>
+#include <stdio.h>
+#include <string.h>
+
+#include <dlt_shm.h>
+
+void dlt_shm_print_hex(char *ptr,int size)
+{
+ int num;
+
+ for (num=0;num<size;num++)
+ {
+ if((num%16)==15)
+ printf("%.2x\n",((unsigned char*)ptr)[num]);
+ else
+ printf("%.2x ",((unsigned char*)ptr)[num]);
+ }
+ printf("\n");
+}
+
+void dlt_shm_pv(int id,int operation)
+{
+ static struct sembuf semaphor;
+
+ semaphor.sem_op = operation;
+ semaphor.sem_flg = SEM_UNDO;
+
+ if(semop(id, &semaphor,1) == -1) {
+ perror("semop");
+ }
+}
+
+int dlt_shm_init_server(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, 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 */
+}
View Lorry Controller Status