msgq.c

USb in User space, a kernel module that use usb layer

/*
 * msgq.c
 *
 * Author : Lionetti Salvatore <[e-mail]>
 * 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 PROVA
void 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