stp.c

stp source code used in switch

  struct sk_buff *skb;
  unsigned char *pucEthernet;
  int iPacketSize;
 
  if(port_no == 0)
   return;

  port_no--; //VIA driver start at 0 
#ifdef STP_DEBUG
  printk("STP: Send out a config BPDU to port %d\n",port_no);  
#endif
  /* Allocate and zero memory for message */
  {
   iPacketSize = 6 /* DA */ + 6 /* SA */ + 2 /* Len */ + 3 /* LLC */ 
      + CONFIG_BPDU_MESSAGE_BYTES;
 
#if 0
   if (iPacketSize < 64) 
    {
     iPacketSize = 64;
    }
#endif
   
   skb=dev_alloc_skb(iPacketSize);  
   if(skb==NULL)
    {
#ifdef STP_DEBUG
     printk("Can't Send Config BPDU!! memory can't be allocated \n");
#endif
     return;
    }
   pucEthernet = skb_put(skb, iPacketSize);
   memset(pucEthernet, 0, iPacketSize);
  }

  /* DA */
  {
   pucEthernet[0] = 0x01;
   pucEthernet[1] = 0x80;
   pucEthernet[2] = 0xC2;
   pucEthernet[3] = 0x00;
   pucEthernet[4] = 0x00;
   pucEthernet[5] = 0x00;
  }
  
  /* SA */
  {
   unsigned char *mac = bridge_mac;

   pucEthernet[6] = mac[0];
   pucEthernet[7] = mac[1];
   pucEthernet[8] = mac[2];
   pucEthernet[9] = mac[3];
   pucEthernet[10] = mac[4];
   pucEthernet[11] = (unsigned char)(mac[5] + (port_no+1)); //MAC Address for port 1 is increased from system
  }
  
  /* length */
  {
   pucEthernet[12] = 0x00;
   pucEthernet[13] = 3 /* LLC */ + CONFIG_BPDU_MESSAGE_BYTES;
  }
  
  /* LLC */
  {
   pucEthernet[14] = 0x42; /* SSAP */
   pucEthernet[15] = 0x42; /* DSAP */
   pucEthernet[16] = 0x03; /* UI   */
  }

  /* Fill in BPDU parameters */
  {
   unsigned char * pucBPDU;

   /* start of BPDU parameters */
   pucBPDU = &(pucEthernet[17]);

   *pucBPDU++ = (unsigned char)(CONFIG_BPDU_PROTOCOL_ID >> 8);
   *pucBPDU++ = (unsigned char)(CONFIG_BPDU_PROTOCOL_ID & 0xFF);
   *pucBPDU++ = (unsigned char)(CONFIG_BPDU_VERSION);
   *pucBPDU++ = (unsigned char)(Config_bpdu_type);

   *pucBPDU   = (unsigned char)(bpdu->topology_change_acknowledgment 
     ? 0x80 : 0x00);
   *pucBPDU  |= (unsigned char)(bpdu->topology_change ? 0x01 : 0x00);
   
   pucBPDU++;
    
   *pucBPDU++ = (unsigned char)((bpdu->root_id.four_octets[0] & 0xFF000000) >> 24);
   *pucBPDU++ = (unsigned char)((bpdu->root_id.four_octets[0] & 0x00FF0000) >> 16);
   *pucBPDU++ = (unsigned char)((bpdu->root_id.four_octets[0] & 0x0000FF00) >> 8);
   *pucBPDU++ = (unsigned char)((bpdu->root_id.four_octets[0] & 0x000000FF) >> 0);

   *pucBPDU++ = (unsigned char)((bpdu->root_id.four_octets[1] & 0xFF000000) >> 24);
   *pucBPDU++ = (unsigned char)((bpdu->root_id.four_octets[1] & 0x00FF0000) >> 16);
   *pucBPDU++ = (unsigned char)((bpdu->root_id.four_octets[1] & 0x0000FF00) >> 8);
   *pucBPDU++ = (unsigned char)((bpdu->root_id.four_octets[1] & 0x000000FF) >> 0);
   
   *pucBPDU++ = (unsigned char)((bpdu->root_path_cost & 0xFF000000) >> 24);
   *pucBPDU++ = (unsigned char)((bpdu->root_path_cost & 0x00FF0000) >> 16);
   *pucBPDU++ = (unsigned char)((bpdu->root_path_cost & 0x0000FF00) >> 8);
   *pucBPDU++ = (unsigned char)((bpdu->root_path_cost & 0x000000FF) >> 0);
   
   *pucBPDU++ = (unsigned char)((bpdu->bridge_id.four_octets[0] & 0xFF000000) >> 24);
   *pucBPDU++ = (unsigned char)((bpdu->bridge_id.four_octets[0] & 0x00FF0000) >> 16);
   *pucBPDU++ = (unsigned char)((bpdu->bridge_id.four_octets[0] & 0x0000FF00) >> 8);
   *pucBPDU++ = (unsigned char)((bpdu->bridge_id.four_octets[0] & 0x000000FF) >> 0);
   
   *pucBPDU++ = (unsigned char)((bpdu->bridge_id.four_octets[1] & 0xFF000000) >> 24);
   *pucBPDU++ = (unsigned char)((bpdu->bridge_id.four_octets[1] & 0x00FF0000) >> 16);
   *pucBPDU++ = (unsigned char)((bpdu->bridge_id.four_octets[1] & 0x0000FF00) >> 8);
   *pucBPDU++ = (unsigned char)((bpdu->bridge_id.four_octets[1] & 0x000000FF) >> 0);

   *pucBPDU++ = (unsigned char)((bpdu->port_id & 0xFF00) >> 8);
   *pucBPDU++ = (unsigned char)((bpdu->port_id & 0x00FF) >> 0);

   *pucBPDU++ = (unsigned char)((bpdu->message_age & 0xFF00) >> 8);
   *pucBPDU++ = (unsigned char)((bpdu->message_age & 0x00FF) >> 0);
   
   *pucBPDU++ = (unsigned char)((bpdu->max_age & 0xFF00) >> 8);
   *pucBPDU++ = (unsigned char)((bpdu->max_age & 0x00FF) >> 0);

   *pucBPDU++ = (unsigned char)((bpdu->hello_time & 0xFF00) >> 8);
   *pucBPDU++ = (unsigned char)((bpdu->hello_time & 0x00FF) >> 0);

   *pucBPDU++ = (unsigned char)((bpdu->forward_delay & 0xFF00) >> 8);
   *pucBPDU++ = (unsigned char)((bpdu->forward_delay & 0x00FF) >> 0);
   (void)pucBPDU;
  }

#ifdef STP_DEBUG
//  printk("Example program receives a packet!! %d %d %d %d %d %d\n",VIA_P(skb)->wSize,VIA_P(skb)->wVid,VIA_P(skb)->byPriority,VIA_P(skb)->bySrcPortId,VIA_P(skb)->byFwdReason,VIA_P(skb)->bIfTagged);
  /* Set parameters to VIA driver */
#endif

//2005/05/04
#if 0
  VIA_P(skb)->wSize = iPacketSize;
  VIA_P(skb)->wVid = 0;
  VIA_P(skb)->byPriority = BPDU_PRIORITY;
  put_unaligned(0x1L << port_no, (unsigned long *) &(VIA_P(skb)->dwDstPortMsk));
  put_unaligned(0, (unsigned long *) &(VIA_P(skb)->dwTagPortMsk));
  skb->dev=dev_get_by_name("eth0");

  dev_queue_xmit(skb);
#endif


	LPortMaskClearAll (&VIA_LP(skb)->dwDstPortMsk);
	LPortMaskSetBit (&VIA_LP(skb)->dwDstPortMsk, port_no);
	LPortMaskClearAll (&VIA_LP(skb)->dwTagPortMsk);

	VIA_LP(skb)->bIfTagged = 0;
	VIA_LP(skb)->wVid = 0;

  VIA_LP(skb)->wSize = iPacketSize;
  VIA_LP(skb)->byPriority = BPDU_PRIORITY;
  skb->dev=dev_get_by_name("eth0");

	mx_TxPkt (skb);

  //free packet
  //TODO??

 } /* send_config_bpdu */


void SendTcnBpdu(Int port_no, Tcn_bpdu *bpdu)
{
  struct sk_buff *skb;
  unsigned char *pucEthernet;
  int iPacketSize;
  
#ifdef STP_DEBUG
  printk("STP: Send out a TCN BPDU to port %d\n",port_no);  
#endif

  if(port_no == 0)
   return;

  port_no--; //VIA driver start at 0

  /* Allocate and zero memory for message */
  {
   iPacketSize = 6 /* DA */ + 6 /* SA */ + 2 /* Len */ + 3 /* LLC */ 
      + TCN_BPDU_MESSAGE_BYTES;

#if 0
   if (iPacketSize < 64) 
    {
      iPacketSize = 64;
    }
#endif

   skb=dev_alloc_skb(iPacketSize);  
   if(skb==NULL)
    {
#ifdef STP_DEBUG
     printk("Can't Send Config BPDU!! memory can't be allocated \n");
#endif
     return;
    }
   pucEthernet = skb_put(skb, iPacketSize);
   memset(pucEthernet, 0, iPacketSize);
  }
  
  /* DA */
  {
   pucEthernet[0] = 0x01;
   pucEthernet[1] = 0x80;
   pucEthernet[2] = 0xC2;
   pucEthernet[3] = 0x00;
   pucEthernet[4] = 0x00;
   pucEthernet[5] = 0x00;
  }
  
  /* SA */
  {
   unsigned char *mac = bridge_mac;

   pucEthernet[6] = mac[0];
   pucEthernet[7] = mac[1];
   pucEthernet[8] = mac[2];
   pucEthernet[9] = mac[3];
   pucEthernet[10] = mac[4];
   pucEthernet[11] = (unsigned char)(mac[5] + (port_no + 1));
  }

  /* length */
  {
   pucEthernet[12] = 0x00;
   pucEthernet[13] = 3 /* LLC */ + TCN_BPDU_MESSAGE_BYTES;
  }
  
  /* LLC */
  {
   pucEthernet[14] = 0x42; /* SSAP */
   pucEthernet[15] = 0x42; /* DSAP */
   pucEthernet[16] = 0x03; /* UI   */
  }
  
  /* Fill in BPDU parameters */
  {
   unsigned char * pucBPDU;
   
   /* start of BPDU parameters */
   pucBPDU = &pucEthernet[17];
   
   /* Fill in the first four bytes */
   pucBPDU[0] = TCN_BPDU_PROTOCOL_ID >> 8;
   pucBPDU[1] = TCN_BPDU_PROTOCOL_ID & 0xFF;
   pucBPDU[2] = TCN_BPDU_VERSION;
   pucBPDU[3] = Tcn_bpdu_type;
  }
  
  /* Set parameters to VIA driver */

//by flash, 2005/05/04
#if 0
  put_unaligned(iPacketSize, (unsigned short *) &(VIA_P(skb)->wSize)); 
  put_unaligned(0, (unsigned short *) &(VIA_P(skb)->wVid));
  VIA_P(skb)->byPriority = BPDU_PRIORITY;
  put_unaligned(0x1L << port_no, (unsigned long *) &(VIA_P(skb)->dwDstPortMsk));
  put_unaligned(0, (unsigned long *) &(VIA_P(skb)->dwTagPortMsk));
  skb->dev=dev_get_by_name("eth0");

  dev_queue_xmit(skb);
#endif

	LPortMaskClearAll (&VIA_LP(skb)->dwDstPortMsk);
	LPortMaskSetBit (&VIA_LP(skb)->dwDstPortMsk, port_no);
	LPortMaskClearAll (&VIA_LP(skb)->dwTagPortMsk);

	VIA_LP(skb)->bIfTagged = 0;
	VIA_LP(skb)->wVid = 0;

  VIA_LP(skb)->wSize = iPacketSize;
  VIA_LP(skb)->byPriority = BPDU_PRIORITY;
  skb->dev=dev_get_by_name("eth0");

	mx_TxPkt (skb);

  //free packet
  //TODO??

} /* send_tcn_bpdu */


void SetPortState(Int port_no, State state)
 {
  
  if(port_no == 0)
   return;

  port_no--; 

  switch (state) 
   {
    case Disabled:
//by flash, 2005/05/04
//    K_StpSetPort(port_no, STP_PORT_DISABLED);
		mx_PortSetStpState (port_no, STP_PORT_DISABLED);
#ifdef STP_DEBUG
                  printk("Setting port #%d in state DISABLED\n",port_no);
#endif
    if(gvrp_stp_port_disabled!=NULL)
     gvrp_stp_port_disabled(port_no);
    if(gmrp_stp_port_disabled!=NULL)
     gmrp_stp_port_disabled(port_no);
    if(igmp_stp_port_disabled!=NULL)
     igmp_stp_port_disabled(port_no);
        break;
    
  case Blocking:
//by flash, 2005/05/04
//    K_StpSetPort(port_no, STP_PORT_BLOCKING);
		mx_PortSetStpState (port_no, STP_PORT_BLOCKING);
#ifdef STP_DEBUG
                  printk("Setting port #%d in state BLOCKING\n",port_no);
#endif
        break;
  case Listening:
//by flash, 2005/05/04
//    K_StpSetPort(port_no, STP_PORT_LISTENING);
		mx_PortSetStpState (port_no, STP_PORT_LISTENING);
#ifdef STP_DEBUG
                  printk("Setting port #%d in state LISTENING\n",port_no);
#endif
        break;
    
  case Learning:
//by flash, 2005/05/04
//    K_StpSetPort(port_no, STP_PORT_LEARNING);
		mx_PortSetStpState (port_no, STP_PORT_LEARNING);
#ifdef STP_DEBUG
                  printk("Setting port #%d in state LEARNING\n",port_no);
#endif
        break;
  case Forwarding:
//by flash, 2005/05/04
//    K_StpSetPort(port_no, STP_PORT_FORWARDING);
		mx_PortSetStpState (port_no, STP_PORT_FORWARDING);
#ifdef STP_DEBUG
                  printk("Setting port #%d in state FORWARDING\n",port_no);
#endif
    if(gvrp_stp_port_forwarding!=NULL)
     gvrp_stp_port_forwarding(port_no);
    if(gmrp_stp_port_forwarding!=NULL)
     gmrp_stp_port_forwarding(port_no);
    if(igmp_stp_port_forwarding!=NULL)
     igmp_stp_port_forwarding(port_no);
//SNMP Bridge MIB
    forward_transition[port_no]++;
        break;
    
  default:
#ifdef STP_DEBUG
                  printk("Illegal spanning tree state");
#endif
                  break;
  }
  
  
 }  

static int stp_init(void)
 {
  int i;
  unsigned long ulFourOctetsHigh = 0x80008030;
  unsigned long ulFourOctetsLow =  0x01030467;

  ulSpanTreeTickScaling = SPAN_TREE_TICK_SCALING; 
  {
   ulFourOctetsHigh=(0x80000000) | (bridge_mac[0] << 8) | (bridge_mac[1]);
   ulFourOctetsLow= (bridge_mac[2] << 24) | (bridge_mac[3] << 16) | (bridge_mac[4] << 8) | (bridge_mac[5]);
  }

  /* bridge_mac is initialized before this point */

  /* init Bridge_data */
  {
   extern Bridge_data bridge_info; /* (8.5.3) */

   bridge_info.designated_root.four_octets[0] = 0; /* (8.5.3.1) */
   bridge_info.designated_root.four_octets[1] = 0; /* (8.5.3.1) */
   bridge_info.root_path_cost           = (Cost)0; /* (8.5.3.2) */
   bridge_info.root_port                = (Int)0;  /* (8.5.3.3) */
   bridge_info.max_age                  = (Time)0; /* (8.5.3.4) */
   bridge_info.hello_time               = (Time)0; /* (8.5.3.5) */
   bridge_info.forward_delay            = (Time)0; /* (8.5.3.6) */
   bridge_info.bridge_id.four_octets[0] = ulFourOctetsHigh; /* (8.5.3.7) */
   bridge_info.bridge_id.four_octets[1] = ulFourOctetsLow;  /* (8.5.3.7) */
   bridge_info.bridge_max_age           = (Time)(20 * 256); /* (8.5.3.8) */
   bridge_info.bridge_hello_time        = (Time)( 2 * 256); /* (8.5.3.9) */
   bridge_info.bridge_forward_delay     = (Time)(15 * 256); /* (8.5.3.10) */
   bridge_info.topology_change_detected = (BOOL)0;      /* (8.5.3.11) */	// 2007-3-22 SteveLin changed (Boolean-->BOOL)
   bridge_info.topology_change          = (BOOL)0;       /* (8.5.3.12) */ // 2007-3-22 SteveLin changed (Boolean-->BOOL)

   bridge_info.topology_change_time     = 
     bridge_info.bridge_max_age 
     + bridge_info.bridge_forward_delay; /* (8.5.3.13) */

   bridge_info.hold_time                = (Time)1*256;          /* (8.5.3.14) */
  }

  /* init IEEE802.1D code */
  initialisation();

  periodical_thread_pid=kernel_thread((int (*)(void *))stp_periodical_thread, NULL, 0);

  spin_lock_init(&stp_ieee_sem);   

  for(i=0;i< STP_PORT_MAX;i++)
   {
    spin_lock(&stp_ieee_sem); /* protect the ieee802.1d code */

    /* make sure the ieee code knows about this port */
    ulMaxNoPorts = STP_PORT_MAX;
    set_port_priority(i + 1, (128 << 8) + (i + 1));

    spin_unlock(&stp_ieee_sem);
   }

  port_thread_pid=kernel_thread((int (*)(void *))stp_port_thread, NULL, 0);

  return 0;
 }

static void stp_cleanup(void)
 {

//  sem_destroy(&stp_ieee_sem);
//  up(&stp_ieee_sem);

 }

/***************Module parameter information*********************/
MODULE_AUTHOR ("PinChuan Flash Liu <[e-mail]>");
MODULE_DESCRIPTION ("VIA 24+2G Protocol example");
MODULE_LICENSE("GPL");


static  char mac[6]={0x01,0x80,0xC2,0x00,0x00,0x00};
static  char mask[6]={0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};
/****************************************************************/
void stp_start()
 {
  int i;

  stp_info.global_enable=1;
  K_StpSetEbl(Enable);
  stp_tick_enable=1;
  for(i=0;i<STP_PORT_MAX;i++)
   {
    set_port_state(i,Disabled);
//SNMP Bridge MIB
    forward_transition[i]=0;
   }
//   disable_port(i + 1);
  restart=STP_PORT_MAX;
//  BIOvRegBitsOn(FWD_CPU_FWD_CFG, 0x02); //enable BPDU forward to CPU
//  mx_register(mac,mask,_stp);
 }

void stp_stop()
 {
  int i;

  stp_info.global_enable=0;
  K_StpSetEbl(Disable);
  stp_tick_enable=0;
  for(i=0;i<STP_PORT_MAX;i++)
   set_port_state(i,Forwarding);
//  BIOvRegBitsOff(FWD_CPU_FWD_CFG, 0x02); //disable BPDU forward to CPU
//  mx_unregister(mac,mask);
 }

static int __init stp_init_module(void)
 {
#if 0 
  printk("============= STP program============\n"); 
  printk("STP program\n");
  printk("===========================================\n");     
#endif

  restart=STP_PORT_MAX;

  num_tcn=0;
  last_tcn=jiffies;
  stp_info.global_enable=0;


  K_SysGetMac(bridge_mac);
  K_MacReverseMac(bridge_mac); //change to network order


  stp_init();

  if(nf_register_sockopt(&stp_sockopts))
   {
    printk(" cannot register sockopt \n");
    return (FALSE);
   }
   
//  stp_stop();
  mx_register(mac,mask,stp);

#if 0
  {
    while(1) {
      sleep(10000);
    }
  }
#endif
  return 0;
 
 } 

/****************************************************************/
static void __exit stp_cleanup_module(void)
 {

  stp_stop();
  stp_cleanup();
  printk("============= STP program ============\n"); 
  printk("Remove STP program\n");
  printk("===========================================\n");     
 
 }

/****************************************************************/
module_init(stp_init_module);
module_exit(stp_cleanup_module);

/*
 * Local variables:
 *  compile-command: "gcc -DMODULE -D__KERNEL__ -I/usr/src/linux/net/inet -Wall -Wstrict-prototypes -O6 -c lc_drv.c `[ -f /usr/include/linux/modversions.h ] && echo -DMODVERSIONS`"
 *  c-indent-level: 4
 *  c-basic-offset: 4
 *  tab-width: 4
 * End:
 */