linedisc.c

C++ 编写的EROS RTOS

void
out_of_write_space(void)
{
  /* this should be called whenever we run out of write space */
  for( ; ; ) {
    if (CMPXCHG(&SHARED.out_of_write_space, 
		SHARED.out_of_write_space,
		1))
      break;
  }
}

uint32_t
doWrite(const char *data, uint32_t len, uint32_t *outLen)
{
  if (len == 0) {
    if (outLen) *outLen = 0;
    return RC_OK;
  }
  while(1) {
    register uint32_t avail = WBUFF_AVAIL_FOR_RESERVE(&SHARED.wrt_buff);

    if (!avail) {
      /* no space at the inn -- return failure */
      DEBUG(write)
	kprintf(KR_OSTREAM,
		"LineDisc: doWrite -- no space available\n");

      out_of_write_space();

      if (outLen) *outLen = 0;
      return RC_OK;
    } else {
      uint32_t retval;

      DEBUG(write)
	kprintf(KR_OSTREAM,
		"LineDisc: doWrite: %d avail\n",
		avail);
    
      
      if (len < avail) avail = len;
      
      DEBUG(write)
	kprintf(KR_OSTREAM,
		"LineDisc: doWrite: writing %d characters\n",
		avail);
    
      
      retval = write_data_to_buffer(&SHARED.wrt_buff,
				    SHARED.outpProcFlags,
				    data,
				    avail);
      if (retval) {
	
	DEBUG(write)
	  kprintf(KR_OSTREAM,
		  "LineDisc: doWrite: successful!\n");

	if (avail != len) {
	  /* we couldn't write everything */
	  out_of_write_space();
	}
	if (outLen) *outLen = avail;
	break;
      }
    }
    DEBUG(write)
      kprintf(KR_OSTREAM,
	      "LineDisc: doWrite unsuccessful. retrying.\n");
    
    /* failed -- loop around again */
  }
  return RC_OK;
}

uint32_t
ProcessRequest(Message *msg)
{

  msg->snd_len = 0;

  msg->snd_w1 = 0u;
  msg->snd_w2 = 0u;
  msg->snd_w3 = 0u;

  msg->snd_key0 = KR_VOID;
  msg->snd_key1 = KR_VOID;
  msg->snd_key2 = KR_VOID;
  msg->snd_key3 = KR_VOID;
  
  switch(msg->rcv_code) {
  case OC_CharSrc_Write:
    DEBUG(write)
      kprintf(KR_OSTREAM,
	      "LineDisc: got a write request (len = %08x)\n",
	      msg->rcv_len);
    doWrite(msg->rcv_data,
	    msg->rcv_len,
	    &msg->snd_w1);
    wakeup_buddy(LD_Wakeup_WriteRequest);
    msg->snd_code = RC_OK;
    break;
  case OC_CharSrc_PostEvent:
    if ((msg->rcv_w1 & (~CharSrc_UserEventMask)) != 0
	|| msg->rcv_w1 == 0u) {
      /* invalid request -- either we got a non-user event, or the
	 event mask was zero. */
      msg->snd_code = RC_RequestError;
      break;
    }

    while (SHARED.postedEvents) {
      wakeup_buddy(LD_Wakeup_EventPosted);
      YIELD(); /* give him a chance to process the events */
    }
    SHARED.postedEvents = msg->rcv_w1;
    msg->snd_code = RC_OK;
    wakeup_buddy(LD_Wakeup_EventPosted);
    break;
    
  case OC_CharSrc_WaitForEvent:

    if (SHARED.readerWants != UINT32_MAX) {
      DEBUG(waitevent)
	kdprintf(KR_OSTREAM, "LineDisc: Got too many waitforevents!\n");
      msg->snd_code = RC_CharSrc_OneWaiterOnly;
      break;
    }
    DEBUG(waitevent)
      kprintf(KR_OSTREAM,
	      "LineDisc: Got waitforevent -- stashing count %u mask 0x%08x\n",
	      msg->rcv_w1, msg->rcv_w2);
    /* squirrel away key in child */
    process_swap_keyreg(KR_BUDDOM,K_READERKEY,KR_RESUME,KR_VOID);
    /* don't invoke it on the way out */
    msg->snd_invKey = KR_VOID;
    
    if (msg->rcv_w1 > DATABUFF_LEN) msg->rcv_w1 = DATABUFF_LEN;
    
    SHARED.readerEventMask = msg->rcv_w2;
    SHARED.readerWants = msg->rcv_w1;
    
    DEBUG(waitevent)
      kdprintf(KR_OSTREAM, "LineDisc: Stashed -- waking buddy\n");
    wakeup_buddy(LD_Wakeup_GotReader);

    break;

  case OC_CharSrc_Control:

    DEBUG(control)
      kdprintf(KR_OSTREAM,"Linedisc: Control %d, %d\n",msg->snd_w1,msg->snd_w2);

    switch (msg->rcv_w1) {
    case LD_Control_GetInpProcessing:
      msg->snd_w1 = SHARED.inpProcFlags;
      msg->snd_code = RC_OK;
      break;
    case LD_Control_SetInpProcessing:
      /* FIXME: check if the flags are valid */
      SHARED.inpProcFlags = msg->rcv_w2;
      wakeup_buddy(LD_Wakeup_ProcFlags);
      msg->snd_code = RC_OK;
      break;

    case LD_Control_GetOutpProcessing:
      msg->snd_w1 = SHARED.outpProcFlags;
      msg->snd_code = RC_OK;
      break;
    case LD_Control_SetOutpProcessing:
      /* FIXME: check if the flags are valid */
      SHARED.outpProcFlags = msg->rcv_w2;
      wakeup_buddy(LD_Wakeup_ProcFlags);
      msg->snd_code = RC_OK;
      break;

    case LD_Control_GetControlChars:
      msg->snd_len = sizeof(SHARED.control_chars[0]) * LD_Num_CC;
      msg->snd_data = (void *)SHARED.control_chars;
      msg->snd_code = RC_OK;
      break;
    case LD_Control_SetControlChars:
      {
	if (msg->rcv_len == sizeof(SHARED.control_chars)) {
	  int idx;
	  cc_type *in = msg->rcv_data;
	  volatile cc_type *out = SHARED.control_chars;
	  
	  for (idx = LD_Num_CC; idx > 0; idx--) {
	    *in++ = *out++;
	  }
	  wakeup_buddy(LD_Wakeup_Chars);

	  msg->snd_code = RC_OK;
	} else {
	  /* incorrectly sized message */
	  msg->snd_code = RC_RequestError;
	}
      }
      break;

#if 0
    case CharSrc_Control_SetTimeout:
    case CharSrc_Control_GetTimeout:
      
      goto def; /* pass on the request */
#endif

    case CharSrc_Control_GetSpecialChars:
      {
	register int idx;
	char *buffp = DATABUFF;
	int len = 0;

	for (idx = 0; idx < 256; idx++) { /* byte-centric! */
	  if (CHECK_CHAR(SHARED.char_mask,idx)) {
	    *buffp++ = 0;
	    *buffp++ = idx;
	    len += 2;
	  }
	}
	msg->snd_data = DATABUFF;
	msg->snd_len = len;
	msg->snd_code = RC_OK;
      }
      break;

    case CharSrc_Control_SetSpecialChars:
      {
	register int idx;
	register int len = msg->rcv_len;
	uint16_t *curp = (uint16_t *)DATABUFF; 

        DEBUG(control)
          kdprintf(KR_OSTREAM,
                   "LineDisc: Setting %d/2 special characters\n",len);
	
        DEBUG(control)
          kprintf(KR_OSTREAM,
                  "LineDisc: Setting special characters");

	if (len % 2 != 0) {
	  /* invalid request length */
          DEBUG(control)
            kdprintf(KR_OSTREAM,
                     " FAILED: not even number of chars\n");

	  msg->snd_code = RC_RequestError;
	}
        DEBUG(control)
          kprintf(KR_OSTREAM,":");

	/* first, set up character mask */
	CLEAR_MASK(SHARED.char_mask);
	for (idx = (len / sizeof(*curp)) ; idx > 0; idx--) {
	  if ((*curp & ~0xFF) != 0) {
	    /* unicode -- throw it away for now */
            DEBUG(control)
	      kdprintf(KR_OSTREAM,
	  	       "\nLineDisc: Hey! non-ASCII unicode stuff in"
	 	       "SetSpecialChars! (0x%04x)\n",*curp);
	    curp++;
	    continue;
	  }
          DEBUG(control)
            kprintf(KR_OSTREAM," 0x%02x",*curp);

	  SET_CHAR(SHARED.char_mask,(uint8_t)*(curp++));
	}
        DEBUG(control)
          kdprintf(KR_OSTREAM,
	           ". Done\n");

	/* now the character mask is set up -- it's my buddy's job
	   from here */
	wakeup_buddy(LD_Wakeup_Chars);
	msg->snd_code = RC_OK;
	break;
      }
    default: 
      DEBUG(control)
        kdprintf(KR_OSTREAM,
                 "Linedisc: Passing control along to KR_CHRSRC\n");

      msg->snd_code = OC_CharSrc_Control;
      msg->snd_w1 = msg->rcv_w1;
      msg->snd_w2 = msg->rcv_w2;
      msg->snd_w3 = msg->rcv_w3;
      
      msg->snd_key0 = msg->rcv_key0;
      msg->snd_key1 = msg->rcv_key1;
      msg->snd_key2 = msg->rcv_key2;
      msg->snd_key3 = msg->rcv_key3; /* pass along the resume key */

      msg->snd_data = msg->rcv_data;
      msg->snd_len = msg->rcv_len;
      
      msg->snd_invKey = KR_CHRSRC; /* pass it along to the charsrc */
      break;
    }
    break;
/*
 * case OC_KeyType:
 *   msg->snd_code = RC_OK;
 *   msg->snd_w1 = AKT_LineDiscipline;
 *   break;
 */
  case OC_Destroy:
    DEBUG(destroy)
      kdprintf(KR_OSTREAM,
	       "LineDisc: Destroying self!\n");
    
    destroy_self(RC_OK, 1); /* destroy, with buddy there */
    /* NOT REACHED */
    break;
  default:
    msg->snd_code = RC_UnknownRequest;
    break;
  }
  return 1;
}

int
main(void)
{
  Message msg;
  
  initialize_linedisc(); /* either successful or destroys me. */
 
  msg.snd_invKey = KR_RESUME;
  msg.snd_key0 = KR_ARG0;
  msg.snd_key1 = KR_VOID;
  msg.snd_key2 = KR_VOID;
  msg.snd_key3 = KR_VOID;
  msg.snd_data = 0;
  msg.snd_len = 0;
  msg.snd_code = 0;
  msg.snd_w1 = 0;
  msg.snd_w2 = 0;
  msg.snd_w3 = 0;

  msg.rcv_key0 = KR_ARG0;
  msg.rcv_key1 = KR_ARG1;
  msg.rcv_key2 = KR_ARG2;
  msg.rcv_key3 = KR_RESUME;
  msg.rcv_data = DATABUFF;
  
  msg.rcv_code = RC_OK;

  do {
    msg.rcv_len = DATABUFF_LEN;

    RETURN(&msg);

    msg.snd_invKey = KR_RESUME;
    
  } while ( ProcessRequest(&msg) );

  return 0;
}