/*
 * lib.c
 *
 * Copyright 2002, Minoru Murashima. All rights reserved.
 * Distributed under the terms of the BSD License.
 *
 * カーネルライブラリー
 */


#include <types.h>
#include <config.h>
#include <lib.h>
#include <time.h>
#include <console.h>
#include <mm.h>
#include <machine/stdarg.h>


/*--------------------------------------------------------------------------
 *
 * Format string.
 *
 *--------------------------------------------------------------------------*/

enum{
	TMP_BUF_SIZE=12,
};


/*
 * 10進数文字列に変換、バッファの後ろから代入していく。
 * parameters : string,buffer,buffer size,flag
 * return : string length
 */
static inline int iToDecimal(int value,char *buf)
{
	char tmp_buf[TMP_BUF_SIZE];
	int i;


	for(i=TMP_BUF_SIZE-1;;--i)
	{
		tmp_buf[i]=value%10+'0';
		if(!(value/=10))break;
	}
	memcpy(buf,&tmp_buf[i],TMP_BUF_SIZE-i);

	return TMP_BUF_SIZE-i;
}


/*
 * 16進数文字列に変換。
 * parameters : string,buffer,buffer size
 * return : string length
 */
static inline int iToBinary(uint value,char *buf)
{
	char tmp_buf[TMP_BUF_SIZE];
	int i;


	for(i=TMP_BUF_SIZE-1;;--i)
	{
		tmp_buf[i]=value%16+'0';
		if(tmp_buf[i]>'9')tmp_buf[i]+='a'-'9'-1;
		if(!(value/=16))break;
	}
	memcpy(buf,&tmp_buf[i],TMP_BUF_SIZE-i);

	return TMP_BUF_SIZE-i;
}


/*
 * PRIVATE
 * 文字列を変換してバッファに保存する。
 * parameters :
 * return : converted string size
 */
static int formatString(const char *str,char *buf,uint *arg)
{
	int ivalue,size;
	int i;


	for(i=0;*str!='\0';++str)
	{
		if(*str=='%')
		{
			switch(*++str)
			{
				case 'd':
					ivalue=*arg++;
					if(ivalue<0)
					{
						buf[i++]='-';
						ivalue*=-1;
					}
					i+=iToDecimal(ivalue,&buf[i]);
					break;
				case 'u':
					ivalue=*arg++;
					i+=iToDecimal(ivalue,&buf[i]);
					break;
				case 'x':
					i+=iToBinary(*arg++,&buf[i]);
					break;
				case 's':
					size=strlen((char*)*arg);
					memcpy(&buf[i],(char*)*arg,size);
					++arg;
					i+=size;
					break;
				case '%':
				default:
					buf[i++]='%';
			}
		}
		else buf[i++]=*str;
	}

	buf[i]='\0';

	return i;
}


/*------------------------------------------------------------------------*/


int (*write_tty)(void*,size_t,size_t);


/*
 * 画面出力関数
 * 引数 : 文字列(%d,%u,%x,%s対応)、値...
 */
void printk(const char *str,...)
{
	char buf[240];
	int size;
	void *arg = &str+1;


	size=formatString(str,buf,arg);
	write_tty(buf,size,0);
}


/*
 * バッファ出力関数
 * 引数 : 文字列(%d,%u,%x,%s対応)、値...
 */
int sprintk(char *buf,const char *str,...)
{
	void *arg = &str+1;


	return formatString(str,buf,arg);
}


/*
 * micro seccond タイマー
 * 引数 : カウント(1microseccond単位)
 */
void micro_timer(uint count)
{
	uint64 end;


	end=rdtsc();
	end+=count*clock_1micro;
	while(rdtsc()<end);
}


/*
 * mili seccond timer
 * param : value of counts(1ms)
 */
void mili_timer(uint count)
{
	uint64 end;


	end=rdtsc();
	end+=count*clock_1m;
	while(rdtsc()<end);
}


/*
 * Convert ascii to integer
 * parameters : string
 * return : integer or 0(引数が数字でない)
 */
int atoi(const char *str)
{
	int rest=0;


	for(;*str!='\0';++str)
	{
		if((*str<'0')||(*str>'9'))return 0;
		rest=rest*10+*str-'0';
	}

	return rest;
}


// ＦｒｅｅＢＳＤカーネル関数
int kvprintf(char const *fmt, void (*func)(int, void*), void *arg, int radix, va_list ap)
{
	int rest;

	rest = sprintk(arg, fmt, *(uint*)ap);
	
	return rest;
}


/***********************************************************************************
 *
 * IOベクタバッファ
 *
 ***********************************************************************************/


/*
 * IOベクタバッファにコピーする
 * return : copy size
 */
int copyIovec(const char *i_data, const int len, struct iovec *iov, const int iovLen, int *m_iovArrey, int *m_iovOffset)
{
	const char *data = i_data;
	int allSize = 0;
	int dataSize = len;
	int iovArrey = *m_iovArrey;
	int iovOffset = *m_iovOffset;

	/* ユーザーバッファにコピー */
	while (iovArrey < iovLen){
		int cpSize = (dataSize <= iov[iovArrey].iov_len - iovOffset)? dataSize : iov[iovArrey].iov_len - iovOffset;
		memcpy(iov[iovArrey].iov_base + iovOffset, data, cpSize);
		allSize += cpSize;
		dataSize -= cpSize;
		iovOffset += cpSize;

		if (iovOffset == iov[iovArrey].iov_len){
			iovOffset = 0;
			++iovArrey;
		}
		if (dataSize == 0){
			break;
		}
		data += cpSize;
	}
	*m_iovArrey = iovArrey;
	*m_iovOffset = iovOffset;
	
	return allSize;
}


// uio構造体用move関数
int uiomove(caddr_t cp, int n, struct uio *uio)
{
	struct iovec *iov;
	u_int cnt;

	while (n > 0 && uio->uio_resid) {
		iov = uio->uio_iov;
		cnt = iov->iov_len;
		if (cnt == 0) {
			uio->uio_iov++;
			uio->uio_iovcnt--;
			continue;
		}
		if (cnt > n){
			cnt = n;
		}

		switch (uio->uio_segflg) {
			case UIO_USERSPACE:
				// 下へ
			case UIO_USERISPACE:
				// 下へ
			case UIO_SYSSPACE:
				if (uio->uio_rw == UIO_READ){
					memcpy(iov->iov_base, cp, cnt);
				}
				else{
					memcpy(cp, iov->iov_base, cnt);
				}
				break;
			case UIO_NOCOPY:
				break;
		}
		iov->iov_base += cnt;
		iov->iov_len -= cnt;
		uio->uio_resid -= cnt;
		uio->uio_offset += cnt;
		cp += cnt;
		n -= cnt;
	}

	return 0;
}


/***************************************************************************************
 *
 *　デバッグ用
 *
 ***************************************************************************************/


#ifdef DEBUG
/*
 * 画面の先頭行からの任意の桁から文字列を表示する
 */
void printDebug(int column,const char *str,...)
{
	char buf[240];
	int size;
	void *arg = &str + 1;


	displayTop(column, "          ", 10);
	size = formatString(str,buf,arg);
	displayTop(column, buf, size);
}
#endif
