mpc85xx.c

qnx powerpc MPC8245的 BSP源文件

	param.sched_priority = cfg->priority;
	pthread_attr_setschedparam (&pattr, &param);
	pthread_attr_setinheritsched (&pattr, PTHREAD_EXPLICIT_SCHED);

	if ((ret = pthread_create (&ext->tid, &pattr,(void *) mpc_driver_thread, ext)) != EOK) {
		errno = ret;
		nic_slogf (_SLOGC_NETWORK, _SLOG_ERROR, "pthread_create at \"%s\":%d: %s\n",
			__FILE__, __LINE__, strerror(errno));
		goto error;
	}
	err++;

	memset (&event, 0, sizeof(event));
	SIGEV_PULSE_INIT (&event, ext->coid, cfg->priority, NIC_INTR_EVENT_TX, 0);

	if ((ext->iid [0] = InterruptAttachEvent (cfg->irq [0], &event, _NTO_INTR_FLAGS_TRK_MSK)) < 0) {
		nic_slogf (_SLOGC_NETWORK, _SLOG_ERROR, "InterruptAttachEvent at \"%s\":%d: %s\n",
			__FILE__, __LINE__, strerror(errno));
		goto error;
	}
	err++;

	memset (&event, 0, sizeof(event));
	SIGEV_PULSE_INIT (&event, ext->coid, cfg->priority, NIC_INTR_EVENT_RX, 0);

	if ((ext->iid [1] = InterruptAttachEvent (cfg->irq [1], &event, _NTO_INTR_FLAGS_TRK_MSK)) < 0) {
		nic_slogf (_SLOGC_NETWORK, _SLOG_ERROR, "InterruptAttachEvent at \"%s\":%d: %s\n",
			__FILE__, __LINE__, strerror(errno));
		goto error;
	}
	err++;

	memset (&event, 0, sizeof(event));
	SIGEV_PULSE_INIT (&event, ext->coid, cfg->priority, NIC_INTR_EVENT_ERR, 0);

	if ((ext->iid [2] = InterruptAttachEvent (cfg->irq [2], &event, _NTO_INTR_FLAGS_TRK_MSK)) < 0) {
		nic_slogf (_SLOGC_NETWORK, _SLOG_ERROR, "InterruptAttachEvent at \"%s\":%d: %s\n",
			__FILE__, __LINE__, strerror(errno));
		goto error;
	}

	return (EOK);

error:

	switch (err) {
		case	6:
			pthread_kill (ext->tid, SIGKILL);
		case	5:
			pthread_mutex_destroy (&ext->rx_free_pkt_q_mutex);
		case	4:
			pthread_mutex_destroy (&ext->tx_mutex);
		case	3:
//			cache_fini (&ext->cachectl);
		case	2:
			ConnectDetach (ext->coid);
		case	1:
			ChannelDestroy (ext->chid);
	}
	return (errno);
}

/****************************************************************************/
/*                                                                          */
/****************************************************************************/

int		mpc_advertise (int reg_hdl, void *func_hdl)

{
	npkt_t					*npkt;
	net_iov_t				*iov;
	mpc85xx_t    		    *ext = (mpc85xx_t *) func_hdl;
	io_net_msg_dl_advert_t	*ap;
	nic_config_t			*cfg = &ext->cfg;


	_mutex_lock (&ext->rx_free_pkt_q_mutex);
	if ((npkt = ext->rx_free_pkt_q)) {
		ext->rx_free_pkt_q = npkt->next;
		npkt->next = NULL;
		ext->num_rx_free--;
		_mutex_unlock (&ext->rx_free_pkt_q_mutex);
	} else {
		_mutex_unlock (&ext->rx_free_pkt_q_mutex);
		if ((npkt = mpc_alloc_npkt(ext, MPC_MTU_SIZE, 1)) == NULL) {
			return (0);
		}
	}

	iov = TAILQ_FIRST(&npkt->buffers)->net_iov;
	ap = iov->iov_base;

	memset (ap, 0x00, sizeof *ap);
	ap->type = _IO_NET_MSG_DL_ADVERT;
	ap->iflags = (IFF_SIMPLEX | IFF_BROADCAST | IFF_RUNNING);
	if (cfg->flags & NIC_FLAG_MULTICAST) {
		ap->iflags |= IFF_MULTICAST;
	} 

	ap->mtu_min = 0;
	ap->mtu_max = cfg->mtu;
	ap->mtu_preferred = cfg->mtu;
	strcpy (ap->up_type, "en");
	itoa (cfg->lan, ap->up_type + 2, 10);

	strcpy (ap->dl.sdl_data, ap->up_type);

	ap->dl.sdl_len = sizeof(struct sockaddr_dl);
	ap->dl.sdl_family = AF_LINK;
	ap->dl.sdl_index  = cfg->lan;
	ap->dl.sdl_type = IFT_ETHER;
	ap->dl.sdl_nlen = strlen(ap->dl.sdl_data); /* not null terminated */
	ap->dl.sdl_alen = 6;
	memcpy (ap->dl.sdl_data + ap->dl.sdl_nlen, cfg->current_address, 6);

	npkt->flags |= _NPKT_MSG;
	npkt->iface = 0;
	npkt->framelen = iov->iov_len = sizeof *ap;
	npkt->tot_iov = 1;
	
	if (ion_add_done (ext->reg_hdl, npkt, ext) == -1) {
		mpc_receive_complete(npkt, ext, ext);
		return (0);
	}

	if(ion_rx_packets (ext->reg_hdl, npkt, 0, 0, ext->cell, cfg->lan, 0) == 0) {
		ion_tx_complete (ext->reg_hdl, npkt);
	}

	return (0);
}

/****************************************************************************/
/*                                                                          */
/*  The Usual Multicast Setup                                               */
/*                                                                          */
/****************************************************************************/

// CRC table (256 entries) generated by RFC3309

unsigned crctab[] = {
0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419,
0x706af48f, 0xe963a535, 0x9e6495a3, 0x0edb8832, 0x79dcb8a4,
0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07,
0x90bf1d91, 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de,
0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, 0x136c9856,
0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9,
0xfa0f3d63, 0x8d080df5, 0x3b6e20c8, 0x4c69105e, 0xd56041e4,
0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b,
0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3,
0x45df5c75, 0xdcd60dcf, 0xabd13d59, 0x26d930ac, 0x51de003a,
0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599,
0xb8bda50f, 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924,
0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, 0x76dc4190,
0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f,
0x9fbfe4a5, 0xe8b8d433, 0x7807c9a2, 0x0f00f934, 0x9609a88e,
0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01,
0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed,
0x1b01a57b, 0x8208f4c1, 0xf50fc457, 0x65b0d9c6, 0x12b7e950,
0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3,
0xfbd44c65, 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2,
0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, 0x4369e96a,
0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5,
0xaa0a4c5f, 0xdd0d7cc9, 0x5005713c, 0x270241aa, 0xbe0b1010,
0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17,
0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, 0xedb88320, 0x9abfb3b6,
0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615,
0x73dc1683, 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8,
0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, 0xf00f9344,
0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb,
0x196c3671, 0x6e6b06e7, 0xfed41b76, 0x89d32be0, 0x10da7a5a,
0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5,
0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1,
0xa6bc5767, 0x3fb506dd, 0x48b2364b, 0xd80d2bda, 0xaf0a1b4c,
0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef,
0x4669be79, 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, 0xc5ba3bbe,
0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31,
0x2cd99e8b, 0x5bdeae1d, 0x9b64c2b0, 0xec63f226, 0x756aa39c,
0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713,
0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b,
0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, 0x86d3d2d4, 0xf1d4e242,
0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1,
0x18b74777, 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c,
0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, 0xa00ae278,
0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7,
0x4969474d, 0x3e6e77db, 0xaed16a4a, 0xd9d65adc, 0x40df0b66,
0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605,
0xcdd70693, 0x54de5729, 0x23d967bf, 0xb3667a2e, 0xc4614ab8,
0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b,
0x2d02ef8d,
};

// crc32core: returns RFC3309 CRC value
unsigned 
crc32core(unsigned char *buf, int len)
{
    unsigned crc = 0xffffffff;
    while(len--) {
        crc = (crc>>8) ^ crctab[(crc ^ *buf)&0xff];
        buf++;
    }
    return crc;
}

// reflect: bit-reverse value, swap 0 for n, 1 for n-1 and so on
unsigned 
reflect(unsigned val, int nbits)
{
    unsigned ret = 0;
    int k;
    for (k=1; k < (nbits+1); k++){
        if (val & 1)
            ret |= 1 << (nbits-k);
        val >>= 1;
    }
    return ret;
}

// bump_macaddr: increment multicast address by one
void
bump_macaddr(uint8_t *mcaddr)
{
    int i;

    for (i = 5; i >= 0; i--) {
        if (mcaddr[i] + 1 < 0x100) {
            mcaddr[i]++;
            while (i++ < 6)
                mcaddr[i] = 0;
            return;
        }
    }
}

void	
setup_mcast_range (mpc85xx_t *ext, unsigned char *da, uint64_t len, int on)
{
	unsigned 	crc;
	unsigned 	lsb;
	unsigned	bitIndex;
	unsigned	regIndex;
	uint32_t	*gaddr_reg = ext->reg + MPC_GADDR0;

	while (len--) {

		crc = crc32core(da, ETHER_ADDR_LEN);
        lsb = reflect(crc, 8);    // reverse bottom 8 bits
        bitIndex = lsb & 0x1f;    // least 5 bits for bit index
        regIndex = lsb >> 5;      // most 3 bits for register index

		if (ext->cfg.verbose) {
			fprintf(stderr,"setup_mcast_range(): mac addr: %X %X %X %X %X %X\n",
				da[0], da[1], da[2], da[3], da[4], da[5]);
			fprintf(stderr,"setup_mcast_range(): crc:      %X\n",crc);
			fprintf(stderr,"setup_mcast_range(): rev lsb:  %X\n",lsb);
			fprintf(stderr,"setup_mcast_range(): regIndex: %d\n",regIndex);
			fprintf(stderr,"setup_mcast_range(): bitIndex: %d\n",bitIndex);
			fprintf(stderr,"setup_mcast_range(): on:       %d\n",on);
		}

        if (on) {  // set bit in indicated register
			ext->gaddr[regIndex] |=  (0x80000000 >> bitIndex);
        } else {   // drop bit in indicated register
			ext->gaddr[regIndex] &= ~(0x80000000 >> bitIndex);
        }

		// if we are not in multicast promiscusous mode, write
		// changed hash mask to TSEC.  When we come out of
        // multicast promiscuous mode, our in-memory copy of
		// the hash mask will be rewritten to TSEC by mcast_prom()

        if (!ext->mcast_nprom) {
            gaddr_reg[regIndex] = ext->gaddr[regIndex];
        }

		bump_macaddr(da);
	}

}

void	
mcast_prom (mpc85xx_t *ext, int enable)
{
	int			 I;
	uint32_t	*gaddr_reg = ext->reg + MPC_GADDR0;

	if (enable) {
		if (++ext->mcast_nprom != 1) {
			return;  // stacked request - already done
		}
        // first enable request - enable all in TSEC
        for(I=0; I<8; I++) {
            gaddr_reg[I] = 0xffffffff;
		}
	} else {  // disable (previous) request
        if (ext->mcast_nprom <= 0) {
			return;  // no underflow allowed during regression testing
        }
        // decrement positive valued counter
		if (--ext->mcast_nprom) {
			return;  // still outstanding stacked enable all requests
		}

		// coming out of multicast promiscuous mode - refresh TSEC     
        for(I=0; I<8; I++) {
            gaddr_reg[I] = ext->gaddr[I];
		}
	}
}

#define CONVERT_MAC_ADDR_2_VAL(x)  \
(x[5] + (x[4]<<8) + (x[3]<<16) + (x[2]<<24) + \
(((unsigned long long)(x[1]))<<32) + (((unsigned long long)x[0])<<40) )

int		
do_multicast (mpc85xx_t *ext, struct _io_net_msg_mcast *mcast)
{
	unsigned long long		start_val;
	unsigned long long   	end_val;
	unsigned long long   	range_length;
	unsigned char   		start[ETHER_ADDR_LEN];
	unsigned char   		end[ETHER_ADDR_LEN];

	if (ext->cfg.verbose) {
		nic_slogf (_SLOGC_NETWORK, _SLOG_INFO, 
		"multicast msg %p - type %x - flags %x", mcast, mcast->type, mcast->flags);
	}

	if (mcast->type == _IO_NET_REMOVE_MCAST) {
		if ((mcast->flags & _IO_NET_MCAST_ALL) == 0) {
			memcpy (start, LLADDR(&mcast->mc_min.addr_dl), ETHER_ADDR_LEN);
			memcpy (end,   LLADDR(&mcast->mc_max.addr_dl), ETHER_ADDR_LEN);

            start_val = CONVERT_MAC_ADDR_2_VAL(start);
            end_val   = CONVERT_MAC_ADDR_2_VAL(end);

            if (start_val > end_val) {
				if (ext->cfg.verbose) {
				    nic_slogf(_SLOGC_NETWORK, _SLOG_ERROR,
			        "devn-mpc85xx: _IO_NET_REMOVE_MCAST: start_val > end_val",
			        mcast->type);
				}
                return EINVAL;
            }

            range_length = end_val - start_val + 1;

            if (range_length > 256) {
				mcast_prom(ext, 0);  // was never added to filter
			} else {
				setup_mcast_range (ext, start, range_length, 0);
			}
        } else {
			mcast_prom(ext, 0);  // remove all multicast
		}
	} else if (mcast->type ==  _IO_NET_JOIN_MCAST) {
		if ((mcast->flags & _IO_NET_MCAST_ALL) == 0) {
			if (nic_ether_mcast_valid(mcast) == -1) {
				return EINVAL;
			}

			memcpy (start, LLADDR(&mcast->mc_min.addr_dl), ETHER_ADDR_LEN);
			memcpy (end,   LLADDR(&mcast->mc_max.addr_dl), ETHER_ADDR_LEN);

            start_val = CONVERT_MAC_ADDR_2_VAL(start);
            end_val   = CONVERT_MAC_ADDR_2_VAL(end);

            if (start_val > end_val) {
				if (ext->cfg.verbose) {
				    nic_slogf(_SLOGC_NETWORK, _SLOG_ERROR,
			        "devn-mpc85xx: _IO_NET_JOIN_MCAST: start_val > end_val",
			        mcast->type);
				}
                return EINVAL;
            }

            range_length = end_val - start_val + 1;

            if (range_length > 256) {
				mcast_prom(ext, 1);  // dont add to filter
			} else {
				setup_mcast_range(ext, start, range_length, 1);
			}
		} else {
			mcast_prom(ext, 1);  // join all multicast
		}
	} else {
		if (ext->cfg.verbose) {
		    nic_slogf(_SLOGC_NETWORK, _SLOG_ERROR,
		        "devn-mpc85xx: message 0x%x not supported",
		        mcast->type);
		}
		return EINVAL;
 	}

	return (EOK);
}