/* * msgq.c * * Author : Lionetti Salvatore <salvatorelionetti@yahoo.it> * License: GPL * * This program is free software; you can redistribute it and/or modify * it under the terms of the GNU General Public License as published by * the Free Software Foundation; either version 2 of the License, or * (at your option) any later version. * * This program is distributed in the hope that it will be useful, * but WITHOUT ANY WARRANTY; without even the implied warranty of * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the * GNU General Public License for more details. * * You should have received a copy of the GNU General Public License * along with this program; if not, write to the Free Software * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */#include<linux/errno.h>#include"msgq.h"#include"util.h" /* To have memcpy().*//* * Normal behavior is PACKET for write(), STREAM for read(). * Is strange to have PACKET mode on for read(), if this level !know format of data (es bluetooth). * Is sufficient to have a maximum size for msg, 4now we !impose such constraint. * Format of message hosted:  * ______________ * |int | char* | * ~~~~~~~~~~~~~~ * We assumed2work in same space (now kernel). */ int spaceQ(struct dataQ_T *q) {	return q->len-q->curw;}void reseQ(struct dataQ_T *q) {	int l;	for (l=0; l<5; l++) {		q->spar[0]=0;		q->curr[0]=0;	}	q->curw=0;}/* * writeQ(): return number of bytes delivered. * 	>0 some bytes written, * 	0  no bytes written * 	<0 some error occur. * * PACKET mode: write exactly len bytes or nothing. * STREAM mode: write as many bytes we can. */int writeQ(struct dataQ_T *q, char *buf, int len) {	int ritorno=0;	int space=spaceQ(q);	switch (q->dataOrgWrite) {		case PACKET: 			if (len+4<=space)				ritorno=len;			else				q->lostW++;			break;		case STREAM: 			ritorno=len;			if (ritorno>space)				ritorno=space;			break;		default:			ritorno=-EBADMSG;			break;	}	if (ritorno>0) {		if (q->dataOrgWrite==PACKET) {			*(int*)(q->data+q->curw)=ritorno;			q->curw+=sizeof(int);		}				if (q->wrSrc==USER && q->wrDst==KERN) {			if (CopyFromUser(q->data+q->curw,buf,ritorno))				ritorno = -EFAULT;		} else 	if (q->wrSrc==KERN && q->wrDst==USER) {			if (CopyToUser(  q->data+q->curw,buf,ritorno))				ritorno = -EFAULT;		} else {			memcpy(          q->data+q->curw,buf,ritorno);		}		if (ritorno>0)			q->curw+=ritorno;	}			return ritorno;}int availQ(struct dataQ_T* q, int whoR) {	return q->curw - q->curr[whoR];}/*  * readQ(): return number of bytes readed. * * PACKET mode: read a msg | return 0 (-EAGAIN?); * STREAM mode: read as many bytes we can * read at most len bytes. * Do a copy. * * return error only if some assertion fail. */int readQ(struct dataQ_T *q, int whoR, char *buf, int len) {	int ritorno, avail;	int firstFragment, msgL; /* For PACKET only.*/	firstFragment=1; msgL=0;	avail=availQ(q,whoR);	ritorno=0;	if (avail>0 && len>0) {		switch (q->dataOrgWrite/*Read[whoR]*/) {			case PACKET:				/* spare[]!=0 => curr[] ! point to msgLen, but inside msg.*/				if (q->spar[whoR])					firstFragment=0;				ritorno=msgL=*(int*)(&q->data[q->curr[whoR] - q->spar[whoR] - (int)(firstFragment?0:4)]);				if (ritorno<0 || ritorno<q->spar[whoR])					ritorno=-EBADMSG;				else {					ritorno-=q->spar[whoR];					if (ritorno>len)						ritorno=len;					q->curr[whoR]+=firstFragment?4:0; /* skip message len for PACKETwriteMode.*/					q->spar[whoR]+=ritorno;					if (q->spar[whoR]==msgL)						q->spar[whoR]=0;				}				break;			case STREAM:				ritorno=avail;				if (avail>len)					ritorno=len;				break;			default:				ritorno=-EFAULT;				break;		}	}	if (ritorno>0) {		if (q->rdSrc==USER && q->rdDst==KERN) {			if (CopyFromUser(buf,q->data + q->curr[whoR],ritorno))				ritorno = -EFAULT;		} else 	if (q->rdSrc==KERN && q->rdDst==USER) {			if (CopyToUser  (buf,q->data + q->curr[whoR],ritorno))				ritorno = -EFAULT;		} else {			memcpy(		 buf,q->data + q->curr[whoR],ritorno);		}		if (ritorno>0) {			q->curr[whoR]+=ritorno;			if (whoR==0 && q->curr[whoR]==q->curw)				reseQ(q);		}	}	return ritorno;}/*  * read at most len bytes. * Only return pointer. */int readQ_0C(struct dataQ_T *q, int whoR, char **buf, int len) {	return 0;}void initQ(struct dataQ_T *q, char *buf, int len) {	memset(q, 0, sizeof(struct dataQ_T));	q->data=buf;	q->len=len;	/* STREAM is default.*/	q->dataOrgWrite=PACKET;#ifdef __KERNEL__	q->wrSrc = USER; q->wrDst = KERN;	q->rdSrc = KERN; q->rdDst = USER;#else	q->wrSrc = q->wrDst = q->rdSrc = q->rdDst = USER;#endif       	}void copyQ(struct dataQ_T *q, struct dataQ_T *q2copy) {	memcpy(q, q2copy, sizeof(struct dataQ_T));}static char* getAddrSpaceStr(enum AddrSpaceT as) {	return as==USER?"USER":"KERN";}int dumpQ(struct dataQ_T *q, char* buf, int len) {	int ritorno=0;	char* curr;	int nOfMsg=0;	int l=0;	Snprintf("\n(data=%p len=%d curw=%d curr={%d,%d,%d,%d,%d}",q->data,q->len,q->curw,q->curr[0],q->curr[1],q->curr[2],q->curr[3],q->curr[4]);	Snprintf(" spar={%d,%d,%d,%d,%d})",q->spar[0],q->spar[1],q->spar[2],q->spar[3],q->spar[4]);	Snprintf("\nDumping dataQ(%d bytes) lostR=%d, lostW=%d",q->curw-q->curr[0], q->lostR, q->lostW);	Snprintf("wr(%s->%s) rd(%s->%s).",getAddrSpaceStr(q->wrSrc),getAddrSpaceStr(q->wrDst),getAddrSpaceStr(q->rdSrc),getAddrSpaceStr(q->rdDst));	for (curr=q->data; (curr<q->data+q->curw) && ritorno>=0;) { 		int msgL;		switch (q->dataOrgWrite) {			case STREAM:				msgL=q->curw-q->curr[0];				break;			case PACKET:				msgL=*((int*)curr);				curr+=sizeof(int);				break;			default:				msgL=0;				ritorno=-EFAULT;				break;						}		if (ritorno>=0) {			int n;			Snprintf("\n   %d) len=%d data@%p=",nOfMsg++,msgL,curr);			for (n=0; n<msgL; n++)				Snprintf("%02x ",*((unsigned char*)curr++));		}	}	if (ritorno<0)		Snprintf(" DATAQ_T: assert failed, object@0x%x overwritten!!!\n",(unsigned int)q);	return ritorno;}void testQ(void) {	char des[4000];	char data[10];	int  desL=4000;	int  dataL=10;	int  l;	for (l=0; l<dataL; l++)		data[l]=l;	{		struct dataQ_T pq;		char data2[30];		int  data2L=30;		int  rit=0;		initQ(&pq,data2,data2L);		dumpQ(&pq,des,desL); PRINT("\n\nINIT:%s\n",des);		rit=writeQ(&pq,data,dataL);		dumpQ(&pq,des,desL); PRINT("\n\nWRITE(%d/10):%s",rit,des);		rit=writeQ(&pq,data,dataL);		dumpQ(&pq,des,desL); PRINT("\n\nWRITE(%d/10):%s",rit,des);		rit=readQ(&pq,0,data,3);		dumpQ(&pq,des,desL); PRINT("\n\nREAD(%d/3):%s",rit,des);		PRINT("\nbytesReaded:");		for (l=0; l<rit; l++)			PRINT("%02x",data[l]);		rit=readQ(&pq,0,data,4);		dumpQ(&pq,des,desL); PRINT("\n\nREAD(%d/4):%s",rit,des);		PRINT("\nbytesReaded:");		for (l=0; l<rit; l++)			PRINT("%02x",data[l]);		rit=readQ(&pq,0,data,10);		dumpQ(&pq,des,desL); PRINT("\n\nREAD(%d/10):%s",rit,des);		PRINT("\nbytesReaded:");		for (l=0; l<rit; l++)			PRINT("%02x",data[l]);		rit=readQ(&pq,0,data,0);		dumpQ(&pq,des,desL); PRINT("\n\nREAD(%d/0):%s",rit,des);		PRINT("\nbytesReaded:");		for (l=0; l<rit; l++)			PRINT("%02x",data[l]);		rit=readQ(&pq,0,data,9);		dumpQ(&pq,des,desL); PRINT("\n\nREAD(%d/9):%s",rit,des);		PRINT("\nbytesReaded:");		for (l=0; l<rit; l++)			PRINT("%02x",data[l]);		rit=readQ(&pq,0,data,9);		dumpQ(&pq,des,desL); PRINT("\n\nREAD(%d/9):%s",rit,des);		PRINT("\nbytesReaded:");		for (l=0; l<rit; l++)			PRINT("%02x",data[l]);		rit=readQ(&pq,0,data,2);		dumpQ(&pq,des,desL); PRINT("\n\nREAD(%d/2):%s",rit,des);		PRINT("\nbytesReaded:");		for (l=0; l<rit; l++)			PRINT("%02x",data[l]);		reseQ(&pq);		dumpQ(&pq,des,desL); PRINT("\n\nRESET:%s",des);	}}#ifdef PROVAvoid consuma(struct dataQ_T* dq, int numbytes) {	char des[4000];	int ret=0;	static int valueVer=0;	int l;	printf("\nReading %d bytes...",numbytes);	if (numbytes>0)	       ret=readQ(dq,0,des,numbytes);	printf("ret(%d)",ret);		for (l=0; l<ret; l++)		if (valueVer++!=des[l])			printf("Expected %02x, got %02x!!!",valueVer-1,des[l]);	dumpQ(dq,des,4000); printf("%s\n",des);}void produci(struct dataQ_T* dq, int numbytes) {	char des[4000];	int ret=0;	static int valueGen=0;	int l;	for (l=0; l<numbytes; l++)		des[l]=valueGen++;	if (numbytes)		ret=writeQ(dq,des,numbytes);	printf("\nWriting %d bytes...ret(%d)",numbytes,ret);	dumpQ(dq,des,4000); printf("%s\n",des);}int main(int argc, char** argv) {	struct dataQ_T dq;	char str[20];	initQ(&dq,malloc(30),30);	if (1) {		produci(&dq,12);		produci(&dq,13);		produci(&dq,12);		produci(&dq,13);		consuma(&dq,3072);		consuma(&dq,3072);		consuma(&dq,3072);		consuma(&dq,3072);	} else {		consuma(&dq,1024);		produci(&dq,14);		consuma(&dq,1024);		produci(&dq,14);		produci(&dq,14);		consuma(&dq,2);		consuma(&dq,12);		produci(&dq,14);		consuma(&dq,14);		consuma(&dq,20);		produci(&dq,4);		produci(&dq,4);		produci(&dq,4);		consuma(&dq,4);		consuma(&dq,4);		consuma(&dq,4);	}	return 0;}#endif