Merged version for SHM now works.

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 */
+}