From 43007289af347bff5aa9fee7af628e9bf17d1d3a Mon Sep 17 00:00:00 2001 From: Alexander Wenzel Date: Thu, 10 Nov 2011 23:58:00 +0100 Subject: Reworkd SHM implementation. --- src/shared/dlt_shm.c | 500 +++++++++++++++++++++++++-------------------------- 1 file changed, 244 insertions(+), 256 deletions(-) (limited to 'src/shared/dlt_shm.c') diff --git a/src/shared/dlt_shm.c b/src/shared/dlt_shm.c index 5ad4775..59915ba 100644 --- a/src/shared/dlt_shm.c +++ b/src/shared/dlt_shm.c @@ -188,11 +188,11 @@ int dlt_shm_init_client(DltShm *buf,int key) { return -1; /* ERROR */ } - // Now we attach the segment to our data space. - if ((buf->shm = shmat(buf->shmid, NULL, 0)) == (char *) -1) { - dlt_log(LOG_ERR,"shmat"); - return -1; /* ERROR */ - } + // Now we attach the segment to our data space. + if ((buf->shm = shmat(buf->shmid, NULL, 0)) == (char *) -1) { + dlt_log(LOG_ERR,"shmat"); + return -1; /* ERROR */ + } // Init semaphore if( (buf->semid = semget(DLT_SHM_SEM,0,0)) == -1 ) { @@ -201,12 +201,9 @@ int dlt_shm_init_client(DltShm *buf,int key) { } // Init pointers - buf->mem = (char*)(&(((int*)(buf->shm))[3])); - buf->size = shm_buf.shm_segsz - (buf->mem - buf->shm); + buf->mem = (char*)(&(((int*)(buf->shm))[3])); + buf->size = shm_buf.shm_segsz - (buf->mem - buf->shm); - //dlt_shm_status(buf); - //dlt_shm_info(buf); - return 0; /* OK */ } @@ -271,13 +268,40 @@ int dlt_shm_get_message_count(DltShm *buf) return ((int*)(buf->shm))[2]; } +void dlt_shm_write_block(DltShm *buf,int *write, const unsigned char *data,unsigned int size) +{ + if((*write+size) <= buf->size) { + // write one block + memcpy(buf->mem+*write,data,size); + *write += size; + } + else { + // write two blocks + memcpy(buf->mem+*write, data, buf->size-*write); + memcpy(buf->mem, data+buf->size-*write, size-buf->size+*write); + *write += size-buf->size; + } +} + +void dlt_shm_read_block(DltShm *buf,int *read,unsigned char *data,unsigned int size) +{ + if((*read+size) <= buf->size) { + // read one block + memcpy(data,buf->mem+*read,size); + *read += size; + } + else { + // read two blocks + memcpy(data, buf->mem+*read, buf->size-*read); + memcpy(data+buf->size-*read, buf->mem, size-buf->size+*read); + *read += size-buf->size; + } +} int dlt_shm_push(DltShm *buf,const unsigned char *data1,unsigned int size1,const unsigned char *data2,unsigned int size2,const unsigned char *data3,unsigned int size3) { - int write, read, count; - char head[] = DLT_SHM_HEAD; - - // initialise head - head[3] = 0x01; + int free_size; + int write, read; + DltShmBlockHead head; if(!buf->mem) { // shm not initialised @@ -291,69 +315,42 @@ int dlt_shm_push(DltShm *buf,const unsigned char *data1,unsigned int size1,const // get current write pointer write = ((int*)(buf->shm))[0]; read = ((int*)(buf->shm))[1]; - count = ((int*)(buf->shm))[2]; + + // check pointers + if((read>buf->size) || (write>buf->size)) + { + dlt_log(LOG_ERR,"SHM: Pointer out of range\n"); + dlt_shm_reset(buf); + return -1; // ERROR + } + + // calculate free size + if(read>write) + free_size = read - write; + else + free_size = buf->size - write + read; - // check space and write pointer - if(read==write && count) { - // shm buffer is full + // check size + if(free_size < (sizeof(DltShmBlockHead)+size1+size2+size3)) { DLT_SHM_SEM_FREE(buf->semid); - dlt_log(LOG_ERR,"SHM is totally full\n"); + dlt_log(LOG_ERR,"SHM: SHM is full\n"); return -1; // ERROR } - else if(write >= buf->size) { - if((size1+size2+size3+sizeof(head)+sizeof(unsigned char)+sizeof(int)) > read) { - DLT_SHM_SEM_FREE(buf->semid); - dlt_log(LOG_ERR,"SHM is full at start\n"); - return -1; // ERROR - } - write = 0; - } - else if(read > write) { - if((write + size1+size2+size3+sizeof(head)+sizeof(unsigned char)+sizeof(int)) > read) { - DLT_SHM_SEM_FREE(buf->semid); - dlt_log(LOG_ERR,"SHM is full at end\n"); - return -1; // ERROR - } - } - else // read <= write - { - if((write+size1+size2+size3+sizeof(head)+sizeof(unsigned char)+sizeof(int)) > buf->size) { - // data does not fit at end of buffer - // try write at beginning - if((size1+size2+size3+sizeof(head)+sizeof(unsigned char)+sizeof(int)) > read) { - DLT_SHM_SEM_FREE(buf->semid); - dlt_log(LOG_ERR,"SHM is full at start\n"); - return -1; // ERROR - } - // write zero status and size at end if possible - 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+sizeof(head); // 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+sizeof(head); // set write size - + // set header + strcpy(head.head,DLT_SHM_HEAD); + head.status = 2; + head.size = size1+size2+size3; + // write data - memcpy(buf->mem+write+sizeof(unsigned char)+sizeof(int),head,sizeof(head)); - if(data1) - memcpy(buf->mem+write+sizeof(head)+sizeof(unsigned char)+sizeof(int),data1,size1); - if(data2) - memcpy(buf->mem+write+sizeof(head)+sizeof(unsigned char)+sizeof(int)+size1,data2,size2); - if(data3) - memcpy(buf->mem+write+sizeof(head)+sizeof(unsigned char)+sizeof(int)+size1+size2,data3,size3); - - // update write status - *((unsigned char*)(buf->mem+write)) = 2; + dlt_shm_write_block(buf,&write,(unsigned char*)&head,sizeof(DltShmBlockHead)); + if(size1) dlt_shm_write_block(buf,&write,data1,size1); + if(size2) dlt_shm_write_block(buf,&write,data2,size2); + if(size3) dlt_shm_write_block(buf,&write,data3,size3); + + // update global shm pointers + ((int*)(buf->shm))[0] = write; // set new write pointer + ((int*)(buf->shm))[2] += 1; // increase counter // free semaphore DLT_SHM_SEM_FREE(buf->semid); @@ -363,13 +360,10 @@ int dlt_shm_push(DltShm *buf,const unsigned char *data1,unsigned int size1,const int dlt_shm_pull(DltShm *buf,unsigned char *data, int max_size) { - int write, read, count, size; - unsigned char status; - char head[] = DLT_SHM_HEAD; + int used_size; + int write, read, count; char head_compare[] = DLT_SHM_HEAD; - - // initialise head - head[3] = 0x01; + DltShmBlockHead head; if(!buf->mem) { // shm not initialised @@ -383,84 +377,90 @@ int dlt_shm_pull(DltShm *buf,unsigned char *data, int max_size) read = ((int*)(buf->shm))[1]; count = ((int*)(buf->shm))[2]; + // check pointers + if((read>buf->size) || (write>buf->size) || (count<0)) + { + dlt_log(LOG_ERR,"SHM: Pointer out of range\n"); + dlt_shm_reset(buf); + return -1; // ERROR + } + // check if data is in there - if(count<=0) { - if((write!=read) || (count<0)) + if(count==0) { + DLT_SHM_SEM_FREE(buf->semid); + if(write!=read) { + dlt_log(LOG_ERR,"SHM: SHM should be empty, but is not\n"); dlt_shm_reset(buf); } - DLT_SHM_SEM_FREE(buf->semid); 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))); + // calculate used size + if(write>read) + used_size = write - read; + else + used_size = buf->size - read + write; + + // first check size + if(used_size < (sizeof(DltShmBlockHead))) { + DLT_SHM_SEM_FREE(buf->semid); + dlt_log(LOG_ERR,"SHM: Size check 1 failed\n"); + dlt_shm_reset(buf); + return -1; // ERROR } - - // check status - if(status != 2 ) { - //printf("Buffer is not fully written\n"); + + // read header + dlt_shm_read_block(buf,&read,(unsigned char*)&head,sizeof(DltShmBlockHead)); + + // check header + if(memcmp((unsigned char*)(head.head),head_compare,sizeof(head_compare))!=0) + { DLT_SHM_SEM_FREE(buf->semid); - dlt_log(LOG_ERR,"Buffer is not fully written\n"); - return -1; // ERROR + dlt_log(LOG_ERR,"SHM: Header head check failed\n"); + dlt_shm_reset(buf); + return -1; // ERROR } - - // plausibility check of buffer size - if( (read+size) > buf->size) { + if(head.status != 2) + { DLT_SHM_SEM_FREE(buf->semid); - dlt_log(LOG_ERR,"SHM: Buffers size bigger than shm buffer\n"); + dlt_log(LOG_ERR,"SHM: Header status check failed\n"); dlt_shm_reset(buf); - return -1; // ERROR + return -1; // ERROR } - - // check max read size - if(size > max_size) { + + // second check size + if(used_size < (sizeof(DltShmBlockHead)+head.size)) { DLT_SHM_SEM_FREE(buf->semid); - dlt_log(LOG_ERR,"SHM: Buffer is bigger than max size\n"); + dlt_log(LOG_ERR,"SHM: Size check 2 failed\n"); dlt_shm_reset(buf); - return -1; // ERROR + return -1; // ERROR } - - // check head - memcpy(head_compare,buf->mem+read+sizeof(unsigned char)+sizeof(int),sizeof(head)); - if(memcmp(head,head_compare,sizeof(head))!=0) - { - /* SHM message header is not correct */ + + // third check size + if(head.size > max_size) { + dlt_log(LOG_ERR,"SHM: Size check 3 failed\n"); + // nothing to do but data does not fit provided buffer } - - // copy data - memcpy(data,buf->mem+read+sizeof(unsigned char)+sizeof(int)+sizeof(head),size-sizeof(head)); + + // read data + dlt_shm_read_block(buf,&read,data,head.size); // update buffer pointers - ((int*)(buf->shm))[1] = read+sizeof(unsigned char)+sizeof(int)+size; // set new read pointer + ((int*)(buf->shm))[1] = read; // set new read pointer ((int*)(buf->shm))[2] -= 1; // decrease counter DLT_SHM_SEM_FREE(buf->semid); - return size-sizeof(head); // OK + return head.size; // OK } int dlt_shm_copy(DltShm *buf,unsigned char *data, int max_size) { - int write, read, count, size; - unsigned char status; - char head[] = DLT_SHM_HEAD; - - // initialise head - head[3] = 0x01; + int used_size; + int write, read, count; + char head_compare[] = DLT_SHM_HEAD; + DltShmBlockHead head; if(!buf->mem) { // shm not initialised @@ -474,69 +474,88 @@ int dlt_shm_copy(DltShm *buf,unsigned char *data, int max_size) read = ((int*)(buf->shm))[1]; count = ((int*)(buf->shm))[2]; + // check pointers + if((read>buf->size) || (write>buf->size) || (count<0)) + { + dlt_log(LOG_ERR,"SHM: Pointer out of range\n"); + dlt_shm_reset(buf); + return -1; // ERROR + } + // check if data is in there - if(count<=0) { - if((write!=read) || (count<0)) + if(count==0) { + DLT_SHM_SEM_FREE(buf->semid); + if(write!=read) { + dlt_log(LOG_ERR,"SHM: SHM should be empty, but is not\n"); dlt_shm_reset(buf); } - DLT_SHM_SEM_FREE(buf->semid); return -1; // ERROR } + // calculate used size + if(write>read) + used_size = write - read; + else + used_size = buf->size - read + write; - // 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))); + // first check size + if(used_size < (sizeof(DltShmBlockHead))) { + DLT_SHM_SEM_FREE(buf->semid); + dlt_log(LOG_ERR,"SHM: Size check 1 failed\n"); + dlt_shm_reset(buf); + return -1; // ERROR } - - // check status - if(status != 2 ) { + + // read header + dlt_shm_read_block(buf,&read,(unsigned char*)&head,sizeof(DltShmBlockHead)); + + // check header + if(memcmp((unsigned char*)(head.head),head_compare,sizeof(head_compare))!=0) + { DLT_SHM_SEM_FREE(buf->semid); - dlt_log(LOG_ERR,"Buffer is not fully written\n"); - return -1; // ERROR + dlt_log(LOG_ERR,"SHM: Header head check failed\n"); + dlt_shm_reset(buf); + return -1; // ERROR } - - // plausibility check of buffer size - if( (read+size) > buf->size) { + if(head.status != 2) + { DLT_SHM_SEM_FREE(buf->semid); - dlt_log(LOG_ERR,"SHM: Buffers size bigger than shm buffer\n"); + dlt_log(LOG_ERR,"SHM: Header status check failed\n"); dlt_shm_reset(buf); - return -1; // ERROR + return -1; // ERROR } - - // check max read size - if((size-sizeof(head)) > max_size) { + + // second check size + if(used_size < (sizeof(DltShmBlockHead)+head.size)) { DLT_SHM_SEM_FREE(buf->semid); - dlt_log(LOG_ERR,"SHM: Buffer is bigger than max size\n"); + dlt_log(LOG_ERR,"SHM: Size check 2 failed\n"); dlt_shm_reset(buf); - return -1; // ERROR + return -1; // ERROR } - - // copy data - memcpy(data,buf->mem+read+sizeof(unsigned char)+sizeof(int)+sizeof(head),size-sizeof(head)); + + // third check size + if(head.size > max_size) { + dlt_log(LOG_ERR,"SHM: Size check 3 failed\n"); + // nothing to do but data does not fit provided buffer + } + + // read data + dlt_shm_read_block(buf,&read,data,head.size); + + // do not update pointers DLT_SHM_SEM_FREE(buf->semid); - return size-sizeof(head); // OK + return head.size; // OK } int dlt_shm_remove(DltShm *buf) { - int write, read, count, size; - unsigned char status; + int used_size; + int write, read, count; + char head_compare[] = DLT_SHM_HEAD; + DltShmBlockHead head; if(!buf->mem) { // shm not initialised @@ -550,128 +569,97 @@ int dlt_shm_remove(DltShm *buf) read = ((int*)(buf->shm))[1]; count = ((int*)(buf->shm))[2]; + // check pointers + if((read>buf->size) || (write>buf->size) || (count<0)) + { + dlt_log(LOG_ERR,"SHM: Pointer out of range\n"); + dlt_shm_reset(buf); + return -1; // ERROR + } + // check if data is in there - if(count<=0) { - if((write!=read) || (count<0)) + if(count==0) { + DLT_SHM_SEM_FREE(buf->semid); + if(write!=read) { + dlt_log(LOG_ERR,"SHM: SHM should be empty, but is not\n"); dlt_shm_reset(buf); } - DLT_SHM_SEM_FREE(buf->semid); return -1; // ERROR } + // calculate used size + if(write>read) + used_size = write - read; + else + used_size = buf->size - read + write; - // 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))); - } + // first check size + if(used_size < (sizeof(DltShmBlockHead))) { + DLT_SHM_SEM_FREE(buf->semid); + dlt_log(LOG_ERR,"SHM: Size check 1 failed\n"); + dlt_shm_reset(buf); + return -1; // ERROR } - else { - read = 0; - status = *((unsigned char*)(buf->mem+read)); - size = *((int*)(buf->mem+read+sizeof(unsigned char))); + + // read header + dlt_shm_read_block(buf,&read,(unsigned char*)&head,sizeof(DltShmBlockHead)); + + // check header + if(memcmp((unsigned char*)(head.head),head_compare,sizeof(head_compare))!=0) + { + DLT_SHM_SEM_FREE(buf->semid); + dlt_log(LOG_ERR,"SHM: Header head check failed\n"); + dlt_shm_reset(buf); + return -1; // ERROR } - - // check status - if(status != 2 ) { + if(head.status != 2) + { DLT_SHM_SEM_FREE(buf->semid); - dlt_log(LOG_ERR,"Buffer is not fully written\n"); - return -1; // ERROR + dlt_log(LOG_ERR,"SHM: Header status check failed\n"); + dlt_shm_reset(buf); + return -1; // ERROR } - - // plausibility check of buffer size - if( (read+size) > buf->size) { + + // second check size + if(used_size < (sizeof(DltShmBlockHead)+head.size)) { DLT_SHM_SEM_FREE(buf->semid); - dlt_log(LOG_ERR,"SHM: Buffers size bigger than shm buffer\n"); + dlt_log(LOG_ERR,"SHM: Size check 2 failed\n"); dlt_shm_reset(buf); - return -1; // ERROR + return -1; // ERROR } - + + // do not copy data + // update buffer pointers - ((int*)(buf->shm))[1] = read+sizeof(unsigned char)+sizeof(int)+size; // set new read pointer + if( (read+head.size) <= buf->size) + ((int*)(buf->shm))[1] = read+head.size; // set new read pointer + else + ((int*)(buf->shm))[1] = read+head.size-buf->size; // set new read pointer ((int*)(buf->shm))[2] -= 1; // decrease counter DLT_SHM_SEM_FREE(buf->semid); - return size; // OK + return head.size; // OK } int dlt_shm_reset(DltShm *buf) { - dlt_log(LOG_ERR,"SHM: Pointer corrupted; reset triggered.\n"); + dlt_log(LOG_ERR,"SHM: SHM reset triggered.\n"); /* reset pointers and counters */ DLT_SHM_SEM_GET(buf->semid); - ((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 + ((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 DLT_SHM_SEM_FREE(buf->semid); return 0; /* OK */ } int dlt_shm_recover(DltShm *buf) { - int write, read, count; - char head[] = DLT_SHM_HEAD; - - // initialise head - head[3] = 0x01; - dlt_log(LOG_ERR,"SHM: Head not found; try to recover.\n"); - - /* try to find next valid message */ - DLT_SHM_SEM_GET(buf->semid); - - write = ((int*)(buf->shm))[0]; - read = ((int*)(buf->shm))[1]; - count = ((int*)(buf->shm))[2]; - - while(1) { - if(memcmp(buf->mem+read,head,sizeof(head))==0) { - /* HEAD found */ - if(read>=(sizeof(unsigned char)+sizeof(int))) { - /* HEAD not at beginning, recover */ - count--; - break; - } - else { - /* HEAD at beginning, cannot recover */ - count--; - } - } - read++; - if(read==write) { - /* nothing found */ - break; - } - if(read>=buf->size) { - /* end reached, continue at beginning */ - read = 0; - } - } - - if(read==write) { - ((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 - } - else { - ((int*)(buf->shm))[1] = read - sizeof(unsigned char)+sizeof(int); // set new read pointer - if(count<0) - ((int*)(buf->shm))[2] = 0; // decrease counter - else - ((int*)(buf->shm))[2] = count; // decrease counter - } - - DLT_SHM_SEM_FREE(buf->semid); - - return 0; /* OK */ + return -1; /* OK */ } int dlt_shm_free_server(DltShm *buf) { -- cgit v1.2.1