tun.c

实现vpn网关用来建立 IP虚拟隧道

  int p;

  DBG(CE_CONT,"tun: tun_attach_req %p\n", str);

  if(MBLKL(mp) < DL_ATTACH_REQ_SIZE) {
     tundlerrack(wq, mp, DL_ATTACH_REQ, DL_BADPRIM, 0);
     return;
  }

  if(str->state != DL_UNATTACHED) {
     tundlerrack(wq, mp, DL_ATTACH_REQ, DL_OUTSTATE, 0);
     return;
  }

  p = dlp->attach_req.dl_ppa;

  /* Attach stream to a PPA */
  if( p < 0 || p > TUNMAXPPA){
     tundlerrack(wq, mp, dlp->dl_primitive, DL_BADPPA, 0);
     return;
  }

  if( !(ppa = tun_ppa[p]) ){	 
     tundlerrack(wq, mp, dlp->dl_primitive, DL_BADPPA, 0);
     return;
  }
  str->p_next = ppa->p_str;
  ppa->p_str = str;

  str->ppa = ppa;
  str->state = DL_UNBOUND;

  tundlokack(wq, mp, DL_ATTACH_REQ);

  DBG(CE_CONT,"tun: str %p attached to PPA %d \n", str, p);
}

static void tun_detach_req(queue_t *wq, mblk_t *mp)
{
  struct tunstr *tmp, **prev, *str = (struct tunstr *)wq->q_ptr;
  struct tunppa *ppa = str->ppa;

  DBG(CE_CONT,"tun: tun_detach_req %p\n", str);

  if(MBLKL(mp) < DL_DETACH_REQ_SIZE){
     tundlerrack(wq, mp, DL_DETACH_REQ, DL_BADPRIM, 0);
     return;
  }

  if(str->state != DL_UNBOUND){
     tundlerrack(wq, mp, DL_DETACH_REQ, DL_OUTSTATE, 0);
     return;
  }

  /* Unlink from PPA list */
  for(prev = &ppa->p_str; (tmp = *prev); prev = &tmp->p_next)
     if( tmp == str ) break;
  *prev = tmp->p_next;
  DBG(CE_CONT,"tun: str %p detached from PPA %p\n", str, ppa);

  str->ppa = NULL;
  str->state = DL_UNATTACHED;

  tundlokack(wq, mp, DL_DETACH_REQ);
}

static void tun_bind_req(queue_t *wq, mblk_t *mp)
{
  static struct tundladdr dladdr;
  union  DL_primitives  *dlp;
  struct tunstr *str;
  int size, xidtest;
  uint32_t sap;

  str = (struct tunstr *)wq->q_ptr;
  dlp = (union DL_primitives *)mp->b_rptr;

  if(MBLKL(mp) < DL_BIND_REQ_SIZE) {
     tundlerrack(wq, mp, DL_BIND_REQ, DL_BADPRIM, 0);
     return;
  }

  if(str->state != DL_UNBOUND) {
     tundlerrack(wq, mp, DL_BIND_REQ, DL_OUTSTATE, 0);
     return;
  }

  sap = dlp->bind_req.dl_sap;
  xidtest = dlp->bind_req.dl_xidtest_flg;

  DBG(CE_CONT,"tun: tun_bind_req str %p sap %d\n", str, sap);

  if(xidtest){
     tundlerrack(wq, mp, DL_BIND_REQ, DL_NOAUTO, 0);
     return;
  }
  if(sap > ETHERTYPE_MAX) {
     tundlerrack(wq, mp, dlp->dl_primitive, DL_BADSAP, 0);
     return;
  }

  str->sap = sap;
  str->state = DL_IDLE;

  DBG(CE_CONT,"tun: str %p bound to sap %d\n", str, sap);

  dladdr.sap = sap;

  size = sizeof(dl_bind_ack_t) + TUN_ADDR_LEN;
  if( !(mp = tunchmsg(mp, size, M_PCPROTO, DL_BIND_ACK)) ){
     tunerr(wq, ENOSR);
     return;
  }
  dlp = (union DL_primitives *)mp->b_rptr;
  dlp->bind_ack.dl_sap = sap;
  dlp->bind_ack.dl_addr_length = TUN_ADDR_LEN;
  dlp->bind_ack.dl_addr_offset = sizeof (dl_bind_ack_t);
  dlp->bind_ack.dl_max_conind = 0;
  dlp->bind_ack.dl_xidtest_flg = 0;
  bcopy((caddr_t)&dladdr, (caddr_t)(mp->b_rptr + sizeof(dl_bind_ack_t)), TUN_ADDR_LEN);
  qreply(wq, mp);
}

static void tun_unbind_req(queue_t *wq, mblk_t *mp)
{
  struct tunstr *str = (struct tunstr *)wq->q_ptr;

  DBG(CE_CONT,"tun: tun_unbind_req str %p\n", str);

  if(MBLKL(mp) < DL_UNBIND_REQ_SIZE){
     tundlerrack(wq, mp, DL_UNBIND_REQ, DL_BADPRIM, 0);
     return;
  }

  if(str->state != DL_IDLE) {
     tundlerrack(wq, mp, DL_UNBIND_REQ, DL_OUTSTATE, 0);
     return;
  }

  str->state = DL_UNBOUND;
  str->sap = 0;

  tundlokack(wq, mp, DL_UNBIND_REQ);
}

static void tun_promiscon_req(queue_t *wq, mblk_t *mp)
{
  struct tunstr *str = (struct tunstr *)wq->q_ptr;

  DBG(CE_CONT,"tun: tun_promiscon_req str %p\n", str);

  if( MBLKL(mp) < DL_PROMISCON_REQ_SIZE ){
     tundlerrack(wq, mp, DL_PROMISCON_REQ, DL_BADPRIM, 0);
     return;
  }

  switch( ((dl_promiscon_req_t *)mp->b_rptr)->dl_level ){
     case DL_PROMISC_PHYS:
        DBG(CE_CONT,"tun: PROMISC_PHYS str %p\n", str);
        str->flags |= TUN_ALL_PHY;
        break;

     case DL_PROMISC_SAP:
        DBG(CE_CONT,"tun: PROMISC_SAP str %p\n", str);
        str->flags |= TUN_ALL_SAP;
        break;

     case DL_PROMISC_MULTI:
        DBG(CE_CONT,"tun: PROMISC_MULTI str %p\n", str);
        str->flags |= TUN_ALL_MUL;
        break;

     default:
        tundlerrack(wq, mp, DL_PROMISCON_REQ, DL_NOTSUPPORTED, 0);
        return;
  }
  tundlokack(wq, mp, DL_PROMISCON_REQ);
}

static void tun_promiscoff_req(queue_t *wq, mblk_t *mp)
{
  struct tunstr *str = (struct tunstr *)wq->q_ptr;

  DBG(CE_CONT,"tun: tun_promiscoff_req str %p\n", str);

  if(MBLKL(mp) < DL_PROMISCOFF_REQ_SIZE){
     tundlerrack(wq, mp, DL_PROMISCOFF_REQ, DL_BADPRIM, 0);
     return;
  }

  switch( ((dl_promiscoff_req_t *)mp->b_rptr)->dl_level ){
     case DL_PROMISC_PHYS:
        str->flags &= ~TUN_ALL_PHY;
        break;

     case DL_PROMISC_SAP:
        str->flags &= ~TUN_ALL_SAP;
        break;

     case DL_PROMISC_MULTI:
        str->flags &= ~TUN_ALL_MUL;
        break;

     default:
        tundlerrack(wq, mp, DL_PROMISCOFF_REQ, DL_NOTSUPPORTED, 0);
        return;
  }
  tundlokack(wq, mp, DL_PROMISCOFF_REQ);
}

void tun_physaddr_req(queue_t *wq, mblk_t *mp)
{
  union   DL_primitives   *dlp;
  struct  ether_addr addr;
  int size;
#ifdef TUN_DEBUG
  struct tunstr *str = (struct tunstr *)wq->q_ptr;

  DBG(CE_CONT,"tun: tun_physaddr_req str %p\n", str);
#endif

  if( MBLKL(mp) < DL_PHYS_ADDR_REQ_SIZE ){
     tundlerrack(wq, mp, DL_PHYS_ADDR_REQ, DL_BADPRIM, 0);
     return;
  }
  bzero(&addr, sizeof(addr));
  size = sizeof(dl_phys_addr_ack_t) + ETHERADDRL;
  if( !(mp = tunchmsg(mp, size, M_PCPROTO, DL_PHYS_ADDR_ACK)) )
     return;
  dlp = (union DL_primitives *)mp->b_rptr;
  dlp->physaddr_ack.dl_addr_length = ETHERADDRL;
  dlp->physaddr_ack.dl_addr_offset = sizeof(dl_phys_addr_ack_t);
  bcopy(&addr, (caddr_t)(mp->b_rptr + sizeof(dl_phys_addr_ack_t)), ETHERADDRL);
  qreply(wq, mp);
}

static void tun_unitdata_req(queue_t *wq, mblk_t *mp)
{
  struct tunstr *str = (struct tunstr *)wq->q_ptr;
  struct tunppa *ppa = str->ppa; 
  mblk_t *nmp;

  DBG(CE_CONT,"tun: tun_unitdata_req str %p data %d\n", str, msgdsize(mp));

  if(str->state != DL_IDLE || !ppa ){
     tundlerrack(wq, mp, DL_UNITDATA_REQ, DL_OUTSTATE, 0);
     return;
  }

  if( !(nmp = mp->b_cont) ){
     tundlerrack(wq, mp, DL_UNITDATA_REQ, DL_BADDATA, 0);
     return;
  }

  /* Drop unidata_req part of the message */
  mp->b_cont = NULL;
  freemsg(mp);  

  /* Route frame */
  tun_frame(wq, nmp, TUN_QUEUE); 
}

static mblk_t * tun_unitdata_ind(mblk_t *mp, int type)
{
  dl_unitdata_ind_t *ud_ind;
  struct tundladdr *dla;
  mblk_t *nmp;
  int size;

  DBG(CE_CONT,"tun: tun_unitdata_ind \n");

  /* Allocate new mblk */
  size = sizeof(dl_unitdata_ind_t) + TUN_ADDR_LEN + TUN_ADDR_LEN;
  if( !(nmp = allocb(size, BPRI_LO)) ){
     freemsg(mp);
     return NULL;
  }
  DB_TYPE(nmp) = (uint8_t)M_PROTO;
  nmp->b_wptr = nmp->b_datap->db_lim;
  nmp->b_rptr = nmp->b_wptr - size;

  /* Construct DL_UNITDATA_IND message */
  ud_ind = (dl_unitdata_ind_t *)nmp->b_rptr;
  ud_ind->dl_primitive = DL_UNITDATA_IND;
  ud_ind->dl_dest_addr_length = TUN_ADDR_LEN;
  ud_ind->dl_dest_addr_offset = sizeof(dl_unitdata_ind_t);
  ud_ind->dl_src_addr_length = TUN_ADDR_LEN;
  ud_ind->dl_src_addr_offset = sizeof(dl_unitdata_ind_t) + TUN_ADDR_LEN;
  ud_ind->dl_group_address = 0;

  dla = (struct tundladdr *)(nmp->b_rptr + ud_ind->dl_dest_addr_offset);
  dla->sap = (uint16_t)type;
  dla = (struct tundladdr *)(nmp->b_rptr + ud_ind->dl_src_addr_offset);
  dla->sap = (uint16_t)type;

  nmp->b_cont = mp;
  return nmp;
}

static mblk_t * tun_eth_hdr(mblk_t *mp, int type)
{
  mblk_t *nmp;
  int size;

  DBG(CE_CONT,"tun: tun_eht_hdr \n");

  /* Allocate new mblk */
  size = sizeof(struct ether_header);
  if( !(nmp = allocb(size, BPRI_LO)) ){
     freemsg(mp);
     return NULL;
  }
  DB_TYPE(nmp) = (uint8_t)M_DATA;
  nmp->b_wptr += sizeof(struct ether_header);
  bzero(nmp->b_rptr, sizeof(struct ether_header));
  ((struct ether_header *)nmp->b_rptr)->ether_type = htons(type);
  nmp->b_cont = mp;
  return nmp;
}


/* Route frames */
static void tun_frame(queue_t *wq, mblk_t *mp, int q)
{
  struct tunstr *str = (struct tunstr *)wq->q_ptr;
  register struct tunppa *ppa;
  register struct tunstr *tmp;
  mblk_t *nmp;

  if( !(ppa = str->ppa) ){
     /* Stream is not attached to PPA. Ignore frame. */
     DBG(CE_CONT,"tun: unattached str %p, dropping frame\n", str);

     freemsg(mp);
     return; 
  }

  DBG(CE_CONT,"tun: tun_frame str %p PPA %d\n", str, ppa->id);

  /* Check for the sniffers */
  for( tmp=ppa->p_str; tmp; tmp = tmp->p_next ){
     if( SNIFFER(tmp->flags) && canputnext(tmp->rq) ){
	if( !(nmp = dupmsg(mp)) )
	   continue;

  	DBG(CE_CONT,"tun: frame %d -> sniffer %p\n", msgdsize(nmp), tmp);

        if( tmp->flags & TUN_RAW ){
           if( (nmp=tun_eth_hdr(nmp, ETHERTYPE_IP)) ) 
	      putnext(tmp->rq, nmp);
	   continue;
	}          
        if( (nmp=tun_unitdata_ind(nmp, ETHERTYPE_IP)) )
           putnext(tmp->rq, nmp);
     }
  }

  if( !(str->flags & TUN_CONTROL) ){
     /* Data from the Protocol stream send it to 
      * the Control stream */ 
     DBG(CE_CONT,"tun: frame %d -> control str\n", msgdsize(mp));
     if( canputnext(ppa->rq) ){
	putnext(ppa->rq, mp);
     } else {
	if( q == TUN_QUEUE ){
           DBG(CE_CONT,"tun: queueing frame %d\n", msgdsize(mp));
	   putbq(wq, mp);
	} else {
           DBG(CE_CONT,"tun: dropping frame %d\n", msgdsize(mp));
           freemsg(mp);
	}
     }
  } else {
     /* Data from the Control stream.  
      * Route frame to the Protocol streams. */
     for( tmp=ppa->p_str; tmp; tmp = tmp->p_next ){
        if( tmp->sap==ETHERTYPE_IP &&  canputnext(tmp->rq) ){
	   if( !(nmp = dupmsg(mp)) )
	      continue;

  	   DBG(CE_CONT,"tun: frame %d -> proto %p\n", msgdsize(nmp), tmp);

           if( tmp->flags & TUN_RAW ){
              if( (nmp=tun_eth_hdr(nmp, ETHERTYPE_IP)) ) 
	         putnext(tmp->rq, nmp);
	      continue;
	   }          
           if( (nmp=tun_unitdata_ind(nmp, ETHERTYPE_IP)) )
              putnext(tmp->rq, nmp);
	}
     }
     /* Free original message */
     freemsg(mp);
  }
}

static void tun_dlpi(queue_t *wq, mblk_t *mp)
{
  union DL_primitives *dlp = (union DL_primitives *)mp->b_rptr;
  uint32_t prim = dlp->dl_primitive;

  switch( prim ){
     case DL_INFO_REQ:
        tun_info_req(wq, mp);
        break;

     case DL_ATTACH_REQ:
	qwriter(wq, mp, tun_attach_req, PERIM_OUTER);
        break;

     case DL_DETACH_REQ:
	qwriter(wq, mp, tun_detach_req, PERIM_OUTER);
        break;

     case DL_BIND_REQ:
        tun_bind_req(wq, mp);
        break;

     case DL_UNBIND_REQ:
        tun_unbind_req(wq, mp);
        break;

     case DL_PROMISCON_REQ:
	tun_promiscon_req(wq, mp);
	break;

     case DL_PROMISCOFF_REQ:
	tun_promiscoff_req(wq, mp);
	break;

     case DL_PHYS_ADDR_REQ:
	tun_physaddr_req(wq, mp);
	break;

     case DL_ENABMULTI_REQ:
     case DL_DISABMULTI_REQ:
     default:
        tundlerrack(wq, mp, prim, DL_UNSUPPORTED, 0);
        break;
  }
}

static int tunwput(queue_t *wq, mblk_t *mp)
{
  union DL_primitives *dlp;
  uint32_t prim;
#ifdef TUN_DEBUG
  struct tunstr *str = (struct tunstr *)wq->q_ptr;

  DBG(CE_CONT, "tun: tunwput str %p\n", str);
#endif

  switch( DB_TYPE(mp) ){
     case M_DATA:
	tun_frame(wq, mp, TUN_QUEUE);
	break;

     case M_PROTO:
     case M_PCPROTO:
        dlp = (union DL_primitives *)mp->b_rptr;
        prim = dlp->dl_primitive;

        switch( prim ){
	   case DL_UNITDATA_REQ:
              tun_unitdata_req(wq, mp);
              break;
	   default:
	      /* Queue other DLPI messages for wsrv */
	      putq(wq, mp);
	      break;
	}
	break;

     case M_IOCTL:
	qwriter(wq, mp, tun_ioctl, PERIM_OUTER);
	break;

     case M_FLUSH:
	/* Flush queues */
        if(*mp->b_rptr & FLUSHW) {
           flushq(wq, FLUSHALL);
           *mp->b_rptr &= ~FLUSHW;
        }
        if(*mp->b_rptr & FLUSHR)
           qreply(wq, mp);
        else
           freemsg(mp);

	break;

     default:
        freemsg(mp);
        break;
  }
  return 0;
}

static int tunwsrv(queue_t *wq)
{
  mblk_t *mp;
#ifdef TUN_DEBUG
  struct tunstr *str = (struct tunstr *)wq->q_ptr;

  DBG(CE_CONT,"tun: tunwsrv str %p\n", str);
#endif

  while( (mp = getq(wq)) )
     switch( DB_TYPE(mp) ){
 	 case M_DATA:
	    tun_frame(wq, mp, TUN_DROP);
	    break;

	 case M_PROTO:
	 case M_PCPROTO:
	    tun_dlpi(wq, mp);
   	    break;

	 default:
	    freemsg(mp);
	    break;
     }
  return 0;
}