/**
* free, opposite of malloc
*/
void free(void *ptr) {
struct _freeblk *block = INC_PTR(ptr, (-1 * sizeof(size_t)));
if(!ptr) {
/* Allow free(NULL) */
return;
}
block->next = NULL;
block->prev = NULL;
/* zero fill free'd space */
memset(INC_PTR(block, sizeof(size_t)), 0, block->blocklen - sizeof(size_t));
_append_freelist(block);
_printfreelist();
}
/*
* シリアルポートからの1文字受信
*/
static bool_t
serialPort_readChar(CELLCB *p_cellcb, char *p_c)
{
bool_t buffer_empty;
ER ercd;
SVC(loc_cpu(), gen_ercd_sys(p_cellcb));
/*
* 受信バッファから文字を取り出す．
*/
*p_c = VAR_receiveBuffer[VAR_receiveReadPointer];
INC_PTR(VAR_receiveReadPointer, ATTR_receiveBufferSize);
VAR_receiveCount--;
buffer_empty = (VAR_receiveCount == 0U);
/*
* STARTを送信する．
*/
if (VAR_receiveStopped && VAR_receiveCount
<= BUFCNT_START(ATTR_receiveBufferSize)) {
if (!serialPort_sendChar(p_cellcb, FC_START)) {
VAR_receiveFlowControl = FC_START;
}
VAR_receiveStopped = false;
}
SVC(unl_cpu(), gen_ercd_sys(p_cellcb));
ercd = (ER_BOOL) buffer_empty;
error_exit:
return(ercd);
}
/*
* シリアルポートからの送信可能コールバック
*/
void
sio_ierdy_snd(VP_INT exinf)
{
SPCB *spcb;
spcb = (SPCB *) exinf;
if (spcb->rcv_fc_chr != '\0') {
/*
* START/STOP を送信する．
*/
(void) sio_snd_chr(spcb->siopcb, spcb->rcv_fc_chr);
spcb->rcv_fc_chr = '\0';
}
else if (!(spcb->snd_stopped) && spcb->snd_count > 0) {
/*
* 送信バッファ中から文字を取り出して送信する．
*/
(void) sio_snd_chr(spcb->siopcb,
spcb->snd_buffer[spcb->snd_read_ptr]);
INC_PTR(spcb->snd_read_ptr);
if (spcb->snd_count == SERIAL_BUFSZ) {
_syscall(isig_sem(spcb->spinib->snd_semid));
}
spcb->snd_count--;
}
else {
/*
* 送信すべき文字がない場合は，送信可能コールバックを
* 禁止する．
*/
sio_dis_cbr(spcb->siopcb, SIO_ERDY_SND);
}
}
/*
* シリアルポートからの1文字受信
*/
static bool_t
serial_rea_chr(SPCB *p_spcb, char *p_c)
{
bool_t buffer_empty;
ER ercd;
SVC(loc_cpu(), gen_ercd_sys(p_spcb));
/*
* 受信バッファから文字を取り出す．
*/
*p_c = p_spcb->p_spinib->rcv_buffer[p_spcb->rcv_read_ptr];
INC_PTR(p_spcb->rcv_read_ptr, p_spcb->p_spinib->rcv_bufsz);
p_spcb->rcv_count--;
buffer_empty = (p_spcb->rcv_count == 0U);
/*
* STARTを送信する．
*/
if (p_spcb->rcv_stopped && p_spcb->rcv_count
<= BUFCNT_START(p_spcb->p_spinib->rcv_bufsz)) {
if (!serial_snd_chr(p_spcb, FC_START)) {
p_spcb->rcv_fc_chr = FC_START;
}
p_spcb->rcv_stopped = false;
}
SVC(unl_cpu(), gen_ercd_sys(p_spcb));
ercd = (ER_BOOL) buffer_empty;
error_exit:
return(ercd);
}
/*
* シリアルポートへの送信
*/
static BOOL
serial_wri_chr(SPCB *spcb, char c)
{
BOOL buffer_full;
/*
* LF の前に CR を送信する．
*/
if (c == '\n' && (spcb->ioctl & IOCTL_CRLF) != 0) {
if (serial_wri_chr(spcb, '\r')) {
_syscall(wai_sem(spcb->spinib->snd_semid));
}
}
_syscall(loc_cpu());
if (spcb->snd_count == 0 && !(spcb->snd_stopped)
&& serial_snd_chr(spcb, c)) {
/*
* シリアルI/Oデバイスの送信レジスタに文字を入れるこ
* とに成功した場合．
*/
buffer_full = FALSE;
}
else {
/*
* 送信バッファに文字を入れる．
*/
spcb->snd_buffer[spcb->snd_write_ptr] = c;
INC_PTR(spcb->snd_write_ptr);
spcb->snd_count++;
buffer_full = (spcb->snd_count == SERIAL_BUFSZ);
}
_syscall(unl_cpu());
return(buffer_full);
}
/*
* シリアルポートからの送信可能コールバック
*/
void
sio_irdy_snd(intptr_t exinf)
{
SPCB *p_spcb;
p_spcb = (SPCB *) exinf;
if (p_spcb->rcv_fc_chr != '\0') {
/*
* START/STOP を送信する．
*/
(void) sio_snd_chr(p_spcb->p_siopcb, p_spcb->rcv_fc_chr);
p_spcb->rcv_fc_chr = '\0';
}
else if (!(p_spcb->snd_stopped) && p_spcb->snd_count > 0U) {
/*
* 送信バッファ中から文字を取り出して送信する．
*/
(void) sio_snd_chr(p_spcb->p_siopcb,
p_spcb->p_spinib->snd_buffer[p_spcb->snd_read_ptr]);
INC_PTR(p_spcb->snd_read_ptr, p_spcb->p_spinib->snd_bufsz);
if (p_spcb->snd_count == p_spcb->p_spinib->snd_bufsz) {
if (isig_sem(p_spcb->p_spinib->snd_semid) < 0) {
p_spcb->errorflag = true;
}
}
p_spcb->snd_count--;
}
else {
/*
* 送信すべき文字がない場合は，送信可能コールバックを禁止する．
*/
sio_dis_cbr(p_spcb->p_siopcb, SIO_RDY_SND);
}
}
/*
* シリアルポートからの送信可能コールバック（受け口関数）
*/
void
eiSIOCBR_readySend(CELLIDX idx)
{
CELLCB *p_cellcb;
assert(VALID_IDX(idx));
p_cellcb = GET_CELLCB(idx);
if (VAR_receiveFlowControl != '\0') {
/*
* START/STOP を送信する．
*/
(void) cSIOPort_putChar(VAR_receiveFlowControl);
VAR_receiveFlowControl = '\0';
}
else if (!VAR_sendStopped && VAR_sendCount > 0U) {
/*
* 送信バッファ中から文字を取り出して送信する．
*/
(void) cSIOPort_putChar(VAR_sendBuffer[VAR_sendReadPointer]);
INC_PTR(VAR_sendReadPointer, ATTR_sendBufferSize);
if (VAR_sendCount == ATTR_sendBufferSize) {
if (ciSendSemaphore_signal() < 0) {
VAR_errorFlag = true;
}
}
VAR_sendCount--;
}
else {
/*
* 送信すべき文字がない場合は，送信可能コールバックを禁止する．
*/
cSIOPort_disableCBR(SIOSendReady);
}
}
/**
* malloc, using brk/sbrk
* Loop through free list to find larger enough block:
* * should stop when finding an exact match
* * o.w. split the smallest suitable _freeblk into 2 blocks
* * (24+24) <= block->blocklen >= size
*
* Else call brk/sbrk
*
* Returns: NULL if size is 0 or NULL on error
*/
void *malloc(size_t size) {
struct _freeblk *target = NULL;
size_t reqsize = MAX(sizeof(struct _freeblk), size + sizeof(size_t));
void *retptr = NULL;
if(size == 0) {
return NULL;
}
target = _find_freeblk(reqsize);
if(target != NULL) {
if(target->blocklen == reqsize) {
/* User gets a whole block */
_rm_freeblk(target);
retptr = INC_PTR(target, sizeof(size_t));
}
else if(target->blocklen > reqsize) {
/* Gotta split a block */
_split_freeblk(target, reqsize);
retptr = INC_PTR(target, sizeof(size_t));
}
} else {
/* Gotta get more mem */
intptr_t increment = ((reqsize/PAGE_SIZE) + 1) * PAGE_SIZE;
target = sbrk(increment);
if(target == (struct _freeblk*)-1) {
/* errno = ENOMEM; Set by sbrk() */
return NULL;
} else {
target->blocklen = reqsize;
retptr = INC_PTR(target, sizeof(size_t)); /* ptr to return */
target = INC_PTR(target, reqsize); /* ptr to remaining block*/
target->blocklen = increment - reqsize;
target->next = target->prev = NULL;
_append_freelist(target);
}
}
_printfreelist();
return retptr;
}
/**
* Splits off the first section of a _freeblk, if needed.
* @reqsize MAX of user's size and sizeof(struct _freeblk)
*/
void _split_freeblk(struct _freeblk *oldblock, size_t reqsize) {
struct _freeblk *newblock;
size_t newlen = oldblock->blocklen - reqsize;
if(newlen >= sizeof(struct _freeblk)) {
/* we can split it */
newblock = INC_PTR(oldblock, reqsize);
oldblock->blocklen = reqsize;
newblock->blocklen = newlen;
_replace_freeblk(oldblock, newblock);
}
else {
/* we can't split, so rm oldblock */
_rm_freeblk(oldblock);
}
}
/*
* シリアルポートへの1文字送信
*/
static ER_BOOL
serial_wri_chr(SPCB *p_spcb, char c)
{
bool_t buffer_full;
ER ercd, rercd;
/*
* LFの前にCRを送信する．
*/
if (c == '\n' && (p_spcb->ioctl & IOCTL_CRLF) != 0U) {
/*
* 以下のコードは再帰呼出しになっているが，引数cが'\n'の場合に
* 引数cを'\r'として呼び出すことから，この再帰呼出しは2回目の
* 呼び出しで必ず止まる．
*/
SVC(rercd = serial_wri_chr(p_spcb, '\r'), rercd);
if ((bool_t) rercd) {
SVC(rercd = wai_sem(p_spcb->p_spinib->snd_semid),
gen_ercd_wait(rercd, p_spcb));
}
}
SVC(loc_cpu(), gen_ercd_sys(p_spcb));
if (p_spcb->snd_count == 0U && !(p_spcb->snd_stopped)
&& serial_snd_chr(p_spcb, c)) {
/*
* シリアルI/Oデバイスの送信レジスタに文字を入れることに成功し
* た場合．
*/
buffer_full = false;
}
else {
/*
* 送信バッファに文字を入れる．
*/
p_spcb->p_spinib->snd_buffer[p_spcb->snd_write_ptr] = c;
INC_PTR(p_spcb->snd_write_ptr, p_spcb->p_spinib->snd_bufsz);
p_spcb->snd_count++;
buffer_full = (p_spcb->snd_count == p_spcb->p_spinib->snd_bufsz);
}
SVC(unl_cpu(), gen_ercd_sys(p_spcb));
ercd = (ER_BOOL) buffer_full;
error_exit:
return(ercd);
}
/*
* シリアルポートへの1文字送信
*/
static ER_BOOL
serialPort_writeChar(CELLCB *p_cellcb, char c)
{
bool_t buffer_full;
ER ercd, rercd;
/*
* LFの前にCRを送信する．
*/
if (c == '\n' && (VAR_ioControl & IOCTL_CRLF) != 0U) {
/*
* 以下のコードは再帰呼出しになっているが，引数cが'\n'の場合に
* 引数cを'\r'として呼び出すことから，この再帰呼出しは2回目の
* 呼び出しで必ず止まる．
*/
SVC(rercd = serialPort_writeChar(p_cellcb, '\r'), rercd);
if ((bool_t) rercd) {
SVC(rercd = cSendSemaphore_wait(),
gen_ercd_wait(rercd, p_cellcb));
}
}
SVC(loc_cpu(), gen_ercd_sys(p_cellcb));
if (VAR_sendCount == 0U && !VAR_sendStopped
&& serialPort_sendChar(p_cellcb, c)) {
/*
* SIOの送信レジスタに文字を入れることに成功した場合．
*/
buffer_full = false;
}
else {
/*
* 送信バッファに文字を入れる．
*/
VAR_sendBuffer[VAR_sendWritePointer] = c;
INC_PTR(VAR_sendWritePointer, ATTR_sendBufferSize);
VAR_sendCount++;
buffer_full = (VAR_sendCount == ATTR_sendBufferSize);
}
SVC(unl_cpu(), gen_ercd_sys(p_cellcb));
ercd = (ER_BOOL) buffer_full;
error_exit:
return(ercd);
}
/*
* シリアルインタフェースドライバからの未送信文字の取出し
*/
bool_t
enSerialPortManage_getChar(CELLIDX idx, char *p_c)
{
CELLCB *p_cellcb;
if (VALID_IDX(idx)) { /* ポート番号のチェック */
p_cellcb = GET_CELLCB(idx);
if (VAR_openFlag) { /* オープン済みかのチェック */
if (VAR_sendCount > 0U) {
*p_c = VAR_sendBuffer[VAR_sendReadPointer];
INC_PTR(VAR_sendReadPointer, ATTR_sendBufferSize);
VAR_sendCount--;
return(true);
}
}
}
return(false);
}
/*
* シリアルインタフェースドライバからの未送信文字の取出し
*/
bool_t
serial_get_chr(ID portid, char *p_c)
{
SPCB *p_spcb;
if (1 <= portid && portid <= TNUM_PORT) { /* ポート番号のチェック */
p_spcb = get_spcb(portid);
if (p_spcb->openflag) { /* オープン済みかのチェック */
if (p_spcb->snd_count > 0U) {
*p_c = p_spcb->p_spinib->snd_buffer[p_spcb->snd_read_ptr];
INC_PTR(p_spcb->snd_read_ptr, p_spcb->p_spinib->snd_bufsz);
p_spcb->snd_count--;
return(true);
}
}
}
return(false);
}
void _coalesce_freelist(struct _freeblk *start) {
struct _freeblk *curr = start;
if(!curr) {
return;
}
while(curr != NULL) {
if(INC_PTR(curr, curr->blocklen) == curr->next) {
/* New can before-merge with curr to curr->next */
curr->blocklen += curr->next->blocklen;
if(curr->next->next) {
curr->next->next->prev = curr;
}
curr->next = curr->next->next;
continue;
}
curr = curr->next;
}
}
/**
* realloc, standard realloc.
*/
void *realloc(void *ptr, size_t size) {
size_t oldsize, reqsize;
if(!ptr)
return malloc(size);
reqsize = MAX(sizeof(struct _freeblk), size + sizeof(size_t));
oldsize = *(size_t*)INC_PTR(ptr, (-1 * sizeof(size_t)));
if(reqsize == oldsize) {
return ptr;
} else {
void *newptr = malloc(size); /* Should be size not reqsize */
if(!newptr)
return NULL;
memcpy(newptr, ptr, MIN(oldsize, size));
free(ptr);
return newptr;
}
}
/*
* シリアルポートからの受信
*/
static BOOL
serial_rea_chr(SPCB *spcb, char *c)
{
BOOL buffer_empty;
_syscall(loc_cpu());
/*
* 受信バッファから文字を取り出す．
*/
*c = spcb->rcv_buffer[spcb->rcv_read_ptr];
INC_PTR(spcb->rcv_read_ptr);
spcb->rcv_count--;
buffer_empty = (spcb->rcv_count == 0);
/*
* START を送信する．
*/
if (spcb->rcv_stopped && spcb->rcv_count <= BUFCNT_START) {
if (!serial_snd_chr(spcb, FC_START)) {
spcb->rcv_fc_chr = FC_START;
}
spcb->rcv_stopped = FALSE;
}
_syscall(unl_cpu());
/*
* エコーバック処理．
*/
if ((spcb->ioctl & IOCTL_ECHO) != 0) {
_syscall(wai_sem(spcb->spinib->snd_semid));
if (!serial_wri_chr(spcb, *c)) {
_syscall(sig_sem(spcb->spinib->snd_semid));
}
}
return(buffer_empty);
}
/*
* シリアルポートからの受信通知コールバック
*/
void
sio_irdy_rcv(intptr_t exinf)
{
SPCB *p_spcb;
char c;
p_spcb = (SPCB *) exinf;
c = (char) sio_rcv_chr(p_spcb->p_siopcb);
if ((p_spcb->ioctl & IOCTL_FCSND) != 0U && c == FC_STOP) {
/*
* 送信を一時停止する．送信中の文字はそのまま送信する．
*/
p_spcb->snd_stopped = true;
}
else if (p_spcb->snd_stopped && (c == FC_START
|| (p_spcb->ioctl & IOCTL_FCANY) != 0U)) {
/*
* 送信を再開する．
*/
p_spcb->snd_stopped = false;
if (p_spcb->snd_count > 0U) {
c = p_spcb->p_spinib->snd_buffer[p_spcb->snd_read_ptr];
if (serial_snd_chr(p_spcb, c)) {
INC_PTR(p_spcb->snd_read_ptr, p_spcb->p_spinib->snd_bufsz);
if (p_spcb->snd_count == p_spcb->p_spinib->snd_bufsz) {
if (isig_sem(p_spcb->p_spinib->snd_semid) < 0) {
p_spcb->errorflag = true;
}
}
p_spcb->snd_count--;
}
}
}
else if ((p_spcb->ioctl & IOCTL_FCSND) != 0U && c == FC_START) {
/*
* 送信に対してフロー制御している場合，START は捨てる．
*/
}
else if (p_spcb->rcv_count == p_spcb->p_spinib->rcv_bufsz) {
/*
* バッファフルの場合，受信した文字を捨てる．
*/
}
else {
/*
* 受信した文字を受信バッファに入れる．
*/
p_spcb->p_spinib->rcv_buffer[p_spcb->rcv_write_ptr] = c;
INC_PTR(p_spcb->rcv_write_ptr, p_spcb->p_spinib->rcv_bufsz);
if (p_spcb->rcv_count == 0U) {
if (isig_sem(p_spcb->p_spinib->rcv_semid) < 0) {
p_spcb->errorflag = true;
}
}
p_spcb->rcv_count++;
/*
* STOPを送信する．
*/
if ((p_spcb->ioctl & IOCTL_FCRCV) != 0U && !(p_spcb->rcv_stopped)
&& p_spcb->rcv_count
>= BUFCNT_STOP(p_spcb->p_spinib->rcv_bufsz)) {
if (!serial_snd_chr(p_spcb, FC_STOP)) {
p_spcb->rcv_fc_chr = FC_STOP;
}
p_spcb->rcv_stopped = true;
}
}
}
/*
* シリアルポートからの受信通知コールバック（受け口関数）
*/
void
eiSIOCBR_readyReceive(CELLIDX idx)
{
CELLCB *p_cellcb;
char c;
assert(VALID_IDX(idx));
p_cellcb = GET_CELLCB(idx);
c = (char) cSIOPort_getChar();
if ((VAR_ioControl & IOCTL_FCSND) != 0U && c == FC_STOP) {
/*
* 送信を一時停止する．送信中の文字はそのまま送信する．
*/
VAR_sendStopped = true;
}
else if (VAR_sendStopped && (c == FC_START
|| (VAR_ioControl & IOCTL_FCANY) != 0U)) {
/*
* 送信を再開する．
*/
VAR_sendStopped = false;
if (VAR_sendCount > 0U) {
c = VAR_sendBuffer[VAR_sendReadPointer];
if (serialPort_sendChar(p_cellcb, c)) {
INC_PTR(VAR_sendReadPointer, ATTR_sendBufferSize);
if (VAR_sendCount == ATTR_sendBufferSize) {
if (ciSendSemaphore_signal() < 0) {
VAR_errorFlag = true;
}
}
VAR_sendCount--;
}
}
}
else if ((VAR_ioControl & IOCTL_FCSND) != 0U && c == FC_START) {
/*
* 送信に対してフロー制御している場合，START は捨てる．
*/
}
else if (VAR_receiveCount == ATTR_receiveBufferSize) {
/*
* バッファフルの場合，受信した文字を捨てる．
*/
}
else {
/*
* 受信した文字を受信バッファに入れる．
*/
VAR_receiveBuffer[VAR_receiveWritePointer] = c;
INC_PTR(VAR_receiveWritePointer, ATTR_receiveBufferSize);
if (VAR_receiveCount == 0U) {
if (ciReceiveSemaphore_signal() < 0) {
VAR_errorFlag = true;
}
}
VAR_receiveCount++;
/*
* STOPを送信する．
*/
if ((VAR_ioControl & IOCTL_FCRCV) != 0U && !VAR_receiveStopped
&& VAR_receiveCount >= BUFCNT_STOP(ATTR_receiveBufferSize)) {
if (!serialPort_sendChar(p_cellcb, FC_STOP)) {
VAR_receiveFlowControl = FC_STOP;
}
VAR_receiveStopped = true;
}
}
}
/*
* シリアルポートからの受信通知コールバック
*/
void
sio_ierdy_rcv(VP_INT exinf)
{
SPCB *spcb;
char c;
spcb = (SPCB *) exinf;
c = (char) sio_rcv_chr(spcb->siopcb);
if ((spcb->ioctl & IOCTL_FCSND) != 0 && c == FC_STOP) {
/*
* 送信を一時停止する．送信中の文字はそのまま送信する．
*/
spcb->snd_stopped = TRUE;
}
else if (spcb->snd_stopped && (c == FC_START
|| (spcb->ioctl & IOCTL_FCANY) != 0)) {
/*
* 送信を再開する．
*/
spcb->snd_stopped = FALSE;
if (spcb->snd_count > 0) {
c = spcb->snd_buffer[spcb->snd_read_ptr];
if (serial_snd_chr(spcb, c)) {
INC_PTR(spcb->snd_read_ptr);
if (spcb->snd_count == SERIAL_BUFSZ) {
_syscall(isig_sem(spcb->spinib
->snd_semid));
}
spcb->snd_count--;
}
}
}
else if ((spcb->ioctl & IOCTL_FCSND) != 0 && c == FC_START) {
/*
* 送信に対してフロー制御している場合，START は捨てる．
*/
}
else if (spcb->rcv_count == SERIAL_BUFSZ) {
/*
* バッファフルの場合，受信した文字を捨てる．
*/
}
else {
/*
* 受信した文字を受信バッファに入れる．
*/
spcb->rcv_buffer[spcb->rcv_write_ptr] = c;
INC_PTR(spcb->rcv_write_ptr);
if (spcb->rcv_count == 0) {
_syscall(isig_sem(spcb->spinib->rcv_semid));
}
spcb->rcv_count++;
/*
* STOP を送信する．
*/
if ((spcb->ioctl & IOCTL_FCRCV) != 0 && !(spcb->rcv_stopped)
&& (spcb->rcv_count >= BUFCNT_STOP)) {
if (!serial_snd_chr(spcb, FC_STOP)) {
spcb->rcv_fc_chr = FC_STOP;
}
spcb->rcv_stopped = TRUE;
}
}
}
void
resmgr_enqueue_packet(event_bus_line_t *line, char *data, int size)
{
int nidx;
struct _ocb_list *op;
devi_attr_t *attr = line->attr;
pthread_mutex_lock(&line->mutex);
for (op = line->attr->ocb_list; op; op = op->next) {
nidx = INC_PTR(line->head);
if (nidx == op->ocb->read_ptr) {
op->ocb->read_ptr = INC_PTR(op->ocb->read_ptr);
}
}
switch (line->type) {
case DEVI_CLASS_KBD:
memcpy(&line->u.kq[line->head], data, size);
break;
case DEVI_CLASS_REL: {
struct packet_rel *rel = (struct packet_rel *) data;
struct _mouse_packet *mp = &line->u.mq[line->head];
memcpy(&mp->hdr.time, &rel->timestamp, sizeof(struct timespec));
mp->hdr.buttons = rel->buttons;
mp->dx = rel->dx;
mp->dy = rel->dy;
if (verbosity)
printf("EN: x: %d, y: %d, buttons: %x\n",
rel->dx, rel->dy, rel->buttons);
break;
}
case DEVI_CLASS_ABS: {
struct packet_abs *abs = (struct packet_abs *) data;
struct _touch1_packet *tp = &line->u.tq[line->head];
memcpy(&tp->hdr.time, &abs->timestamp, sizeof(struct timespec));
tp->hdr.buttons = abs->buttons;
tp->x = abs->x;
tp->y = abs->y;
break;
}
default:
break;
}
line->head = INC_PTR(line->head);
if (attr->flags & DEVI_NOTIFICATION_ARMED) {
iofunc_notify_trigger(attr->notify, 1, IOFUNC_NOTIFY_INPUT);
attr->flags &= ~DEVI_NOTIFICATION_ARMED;
}
process_wait_queue(line->attr);
pthread_mutex_unlock(&line->mutex);
}
