u8 rtw_validate_bssid(const u8 *bssid)
{
u8 ret = _TRUE;
if (is_zero_mac_addr(bssid)
|| is_broadcast_mac_addr(bssid)
|| is_multicast_mac_addr(bssid)
) {
ret = _FALSE;
}
return ret;
}
u8 rtw_validate_bssid(u8 *bssid)
{
u8 ret = true;
if (is_zero_mac_addr(bssid)
|| is_broadcast_mac_addr(bssid)
|| is_multicast_mac_addr(bssid)
) {
ret = false;
}
return ret;
}
void rtw_macaddr_cfg(u8 *mac_addr)
{
if (mac_addr == NULL)
return;
if ( rtw_initmac )
{
// Users specify the mac address
if (mac_pton(rtw_initmac, mac_addr))
return;
}
else {
if (!is_zero_mac_addr(mac_addr) &&
!is_broadcast_mac_addr(mac_addr))
return;
}
// use default mac addresss
copy_mac_addr(mac_addr, (u8 []){0x00, 0xe0, 0x4c, 0x87, 0x00, 0x00});
DBG_871X("MAC Address from efuse error, assign default one !!!\n");
DBG_871X("rtw_macaddr_cfg MAC Address = "MAC_FMT"\n", MAC_ARG(mac_addr));
}
int32 hwifi_set_ampdu(struct cfg_struct *cfg, struct aggre_cfg_param *param)
{
struct set_start_ampdu_msg *start_msg;
struct set_end_ampdu_msg *end_msg;
struct sk_buff *skb;
uint16 msg_size;
int32 ret;
HWIFI_ASSERT((NULL != cfg) && (NULL != param));
if(param->ampdu_action_type == A_MPDU_START)
{
msg_size = sizeof(struct set_start_ampdu_msg);
if (msg_size > MAX_MSG_LEN)
{
HWIFI_WARNING("Invalid too long start ampdu msg size:%d",msg_size);
return -EFAIL;
}
skb = hwifi_alloc_skb_for_cmd(msg_size);
if (NULL == skb)
{
return -ENOMEM;
}
start_msg = (struct set_start_ampdu_msg *)skb_put(skb, sizeof(struct set_start_ampdu_msg));
/* add msg header */
hwifi_fill_msg_hdr(&start_msg->msg_header, HOST_CMD_CONFIG, msg_size);
/*add ampdu wid */
hwifi_fill_start_ampdu(&start_msg->set_ampdu, param);
if (is_zero_mac_addr(param->mac_addr))
{ /* use user data */
memcpy(start_msg->set_ampdu.ampdu_wid.bssid, param->mac_addr, MAC_LEN);
}
else if (IS_AP(cfg))
{
/* AP mode */
memcpy(start_msg->set_ampdu.ampdu_wid.bssid, cfg->ap_info.sta_mgmt.stations[CONNECTED_STA_PRE].mac, MAC_LEN);
}
else if (IS_STA(cfg))
{
/* STA mode copy mac address fill ba and ampdu struct*/
memcpy(start_msg->set_ampdu.ampdu_wid.bssid, cfg->conn.bss.bssid, MAC_LEN);
}
}
else
{
msg_size = sizeof(struct set_end_ampdu_msg);
if (msg_size > MAX_MSG_LEN)
{
HWIFI_WARNING("Invalid too long end ampdu msg size:%d",msg_size);
return -EFAIL;
}
skb = hwifi_alloc_skb_for_cmd(msg_size);
if (NULL == skb)
{
return -ENOMEM;
}
end_msg = (struct set_end_ampdu_msg *)skb_put(skb, sizeof(struct set_end_ampdu_msg));
/* add msg header */
hwifi_fill_msg_hdr(&end_msg->msg_header, HOST_CMD_CONFIG, msg_size);
/*add ampdu wid */
hwifi_fill_end_ampdu(&end_msg->set_ampdu, param);
if (is_zero_mac_addr(param->mac_addr))
{ /* use user data */
memcpy(end_msg->set_ampdu.bssid, param->mac_addr, MAC_LEN);
}
else if (IS_AP(cfg))
{
/* AP mode */
memcpy(end_msg->set_ampdu.bssid, cfg->ap_info.sta_mgmt.stations[CONNECTED_STA_PRE].mac, MAC_LEN);
}
else if (IS_STA(cfg))
{
/* STA mode copy mac address fill ba and ampdu struct*/
memcpy(end_msg->set_ampdu.bssid, cfg->conn.bss.bssid, MAC_LEN);
}
}
ret = hwifi_send_cmd(cfg, skb);
PRINT_SEND_CMD_RET("set ampdu", ret);
return ret;
}
int32 hwifi_set_ba(struct cfg_struct *cfg, struct aggre_cfg_param *param)
{
struct set_addba_msg *adb_msg;
struct set_delba_msg *del_msg;
struct sk_buff *skb;
uint16 msg_size;
int32 ret;
HWIFI_ASSERT((NULL != cfg) && (NULL != param));
if(param->ba_action_type == MLME_ADDBA_REQ_TYPE)
{
msg_size = sizeof(struct set_addba_msg);
if (msg_size > MAX_MSG_LEN)
{
HWIFI_WARNING("Invalid too long set addba msg size:%d!",msg_size);
return -EFAIL;
}
skb = hwifi_alloc_skb_for_cmd(msg_size);
if (NULL == skb)
{
return -ENOMEM;
}
adb_msg = (struct set_addba_msg *)skb_put(skb, sizeof(struct set_addba_msg));
/* add msg header */
hwifi_fill_msg_hdr(&adb_msg->msg_header, HOST_CMD_CONFIG, msg_size);
/* add ba wid */
hwifi_fill_addba(&adb_msg->set_ba, param);
if (is_zero_mac_addr(param->mac_addr))
{ /* use user data */
memcpy(adb_msg->set_ba.ba_wid.bssid, param->mac_addr, MAC_LEN);
}
else if (IS_AP(cfg))
{
/* AP mode */
memcpy(adb_msg->set_ba.ba_wid.bssid,
cfg->ap_info.sta_mgmt.stations[CONNECTED_STA_PRE].mac,
MAC_LEN);
}
else if (IS_STA(cfg))
{
/* STA mode copy mac address fill ba and ampdu struct*/
memcpy(adb_msg->set_ba.ba_wid.bssid, cfg->conn.bss.bssid, MAC_LEN);
}
}
else
{
msg_size = sizeof(struct set_delba_msg);
if (msg_size > MAX_MSG_LEN)
{
HWIFI_WARNING("Invalid too long delba msg size:%d",msg_size);
return -EFAIL;
}
skb = hwifi_alloc_skb_for_cmd(msg_size);
if (NULL == skb)
{
return -ENOMEM;
}
del_msg = (struct set_delba_msg *)skb_put(skb, sizeof(struct set_delba_msg));
/* add msg header */
hwifi_fill_msg_hdr(&del_msg->msg_header, HOST_CMD_CONFIG, msg_size);
/* add ba wid */
hwifi_fill_delba(&del_msg->set_ba, param);
if (is_zero_mac_addr(param->mac_addr))
{ /* use user data */
memcpy(del_msg->set_ba.ba_wid.bssid, param->mac_addr, MAC_LEN);
}
else if (IS_AP(cfg))
{
/* AP mode */
memcpy(del_msg->set_ba.ba_wid.bssid, cfg->ap_info.sta_mgmt.stations[CONNECTED_STA_PRE].mac, MAC_LEN);
}
else if (IS_STA(cfg))
{
/* STA mode copy mac address fill ba and ampdu struct*/
memcpy(del_msg->set_ba.ba_wid.bssid, cfg->conn.bss.bssid, MAC_LEN);
}
}
ret = hwifi_send_cmd(cfg, skb);
PRINT_SEND_CMD_RET("set ba", ret);
return ret;
}
/*
* Prototype : wl_query_ba_set
* Description : query ba session info
* Input : struct cfg_struct *cfg,struct query_ba_param *param
* Output : NONE
* Return Value : int32
* Calls :
* Called By : hwifi_ioctl_cmd()
*
* History :
* 1.Date : 2013/03/19
* Author : ywx164715
* Modification : Created function
*
*/
int32 wl_query_ba_set(struct cfg_struct *cfg, struct query_ba_param *param)
{
struct sk_buff *skb;
struct set_query_ba_msg *query_ba_msg;
uint16 msg_size;
int32 ret;
HWIFI_DEBUG("Mac address:" MACFMT, MAC2STR(param->mac_addr));
msg_size = sizeof(struct set_query_ba_msg);
skb = hwifi_alloc_skb_for_cmd(msg_size);
if (NULL == skb)
{
return -ENOMEM;
}
/* fill the msg header */
query_ba_msg = (struct set_query_ba_msg*)skb_put(skb, sizeof(struct set_query_ba_msg));
hwifi_fill_msg_hdr(&query_ba_msg->msg_header, HOST_CMD_CONFIG, msg_size);
hwifi_fill_query_ba_wid(&query_ba_msg->set_query_ba, param);
if (is_zero_mac_addr(param->mac_addr))
{ /* use user data */
memcpy(query_ba_msg->set_query_ba.bssid, param->mac_addr, MAC_LEN);
}else if (IS_AP(cfg) && param->flag == 0){ /* AP mode */
memcpy(query_ba_msg->set_query_ba.bssid, cfg->ap_info.sta_mgmt.stations[CONNECTED_STA_PRE].mac, MAC_LEN);
}else if (IS_STA(cfg)){ /* STA mode copy mac address fill ba and ampdu struct*/
memcpy(query_ba_msg->set_query_ba.bssid, cfg->conn.bss.bssid, MAC_LEN);
}
/* init for wait event */
cfg->aggre_info.ba_param_res.ba_sess_res[ZERO_INDEX].id = 0xFE;
ret = hwifi_send_cmd(cfg, skb);
if (SUCC != ret)
{
HWIFI_WARNING("Failed to send aggreagtion mode set msg!");
return -EFAIL;
}
ret = wait_event_interruptible_timeout(cfg->wait_queue, (0xFE != cfg->aggre_info.ba_param_res.ba_sess_res[ZERO_INDEX].id), 5 * HZ);
if (0 == ret)
{
HWIFI_WARNING("Wait_event_queue time out!");
return -EFAIL;
}
else if (ret < 0)
{
HWIFI_WARNING("Wait_event_queue internal error,ret=%d", ret);
return -EFAIL;
}
else if (cfg->aggre_info.ba_param_res.ba_sess_res[ZERO_INDEX].id == 0xFF)
{
HWIFI_WARNING("Cann't find the mac address or no any connected!");
return -EFAIL;
}
memcpy((uint8 *)param, (uint8 *)&cfg->aggre_info.ba_param_res, sizeof(*param));
HWIFI_DEBUG("Succeed to query ba session!");
return SUCC;
}
int nat25_db_handle(_adapter *priv, struct sk_buff *skb, int method)
{
unsigned short protocol;
unsigned char networkAddr[MAX_NETWORK_ADDR_LEN];
if(skb == NULL)
return -1;
if((method <= NAT25_MIN) || (method >= NAT25_MAX))
return -1;
protocol = *((unsigned short *)(skb->data + 2 * ETH_ALEN));
/*---------------------------------------------------*/
/* Handle IP frame */
/*---------------------------------------------------*/
if(protocol == __constant_htons(ETH_P_IP))
{
struct iphdr* iph = (struct iphdr *)(skb->data + ETH_HLEN);
if(((unsigned char*)(iph) + (iph->ihl<<2)) >= (skb->data + ETH_HLEN + skb->len))
{
DEBUG_WARN("NAT25: malformed IP packet !\n");
return -1;
}
switch(method)
{
case NAT25_CHECK:
return -1;
case NAT25_INSERT:
{
//some muticast with source IP is all zero, maybe other case is illegal
//in class A, B, C, host address is all zero or all one is illegal
if (iph->saddr == 0)
return 0;
DEBUG_INFO("NAT25: Insert IP, SA=%08x, DA=%08x\n", iph->saddr, iph->daddr);
__nat25_generate_ipv4_network_addr(networkAddr, &iph->saddr);
//record source IP address and , source mac address into db
__nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
__nat25_db_print(priv);
}
return 0;
case NAT25_LOOKUP:
{
DEBUG_INFO("NAT25: Lookup IP, SA=%08x, DA=%08x\n", iph->saddr, iph->daddr);
#ifdef SUPPORT_TX_MCAST2UNI
if (priv->pshare->rf_ft_var.mc2u_disable ||
((((OPMODE & (WIFI_STATION_STATE|WIFI_ASOC_STATE))
== (WIFI_STATION_STATE|WIFI_ASOC_STATE)) &&
!checkIPMcAndReplace(priv, skb, &iph->daddr)) ||
(OPMODE & WIFI_ADHOC_STATE)))
#endif
{
__nat25_generate_ipv4_network_addr(networkAddr, &iph->daddr);
if (!__nat25_db_network_lookup_and_replace(priv, skb, networkAddr)) {
if (*((unsigned char *)&iph->daddr + 3) == 0xff) {
// L2 is unicast but L3 is broadcast, make L2 bacome broadcast
DEBUG_INFO("NAT25: Set DA as boardcast\n");
set_broadcast_mac_addr(skb->data);
}
else {
// forward unknow IP packet to upper TCP/IP
DEBUG_INFO("NAT25: Replace DA with BR's MAC\n");
if ( is_zero_mac_addr(priv->br_mac) ) {
void netdev_br_init(struct net_device *netdev);
printk("Re-init netdev_br_init() due to br_mac==0!\n");
netdev_br_init(priv->pnetdev);
}
copy_mac_addr(skb->data, priv->br_mac);
}
}
}
}
return 0;
default:
return -1;
}
}
/*---------------------------------------------------*/
/* Handle ARP frame */
/*---------------------------------------------------*/
else if(protocol == __constant_htons(ETH_P_ARP))
{
struct arphdr *arp = (struct arphdr *)(skb->data + ETH_HLEN);
unsigned char *arp_ptr = (unsigned char *)(arp + 1);
unsigned int *sender, *target;
if(arp->ar_pro != __constant_htons(ETH_P_IP))
{
DEBUG_WARN("NAT25: arp protocol unknown (%4x)!\n", htons(arp->ar_pro));
return -1;
}
switch(method)
{
case NAT25_CHECK:
return 0; // skb_copy for all ARP frame
case NAT25_INSERT:
{
DEBUG_INFO("NAT25: Insert ARP, MAC=%02x%02x%02x%02x%02x%02x\n", arp_ptr[0],
arp_ptr[1], arp_ptr[2], arp_ptr[3], arp_ptr[4], arp_ptr[5]);
// change to ARP sender mac address to wlan STA address
copy_mac_addr(arp_ptr, GET_MY_HWADDR(priv));
arp_ptr += arp->ar_hln;
sender = (unsigned int *)arp_ptr;
__nat25_generate_ipv4_network_addr(networkAddr, sender);
__nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
__nat25_db_print(priv);
}
return 0;
case NAT25_LOOKUP:
{
DEBUG_INFO("NAT25: Lookup ARP\n");
arp_ptr += arp->ar_hln;
sender = (unsigned int *)arp_ptr;
arp_ptr += (arp->ar_hln + arp->ar_pln);
target = (unsigned int *)arp_ptr;
__nat25_generate_ipv4_network_addr(networkAddr, target);
__nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
// change to ARP target mac address to Lookup result
arp_ptr = (unsigned char *)(arp + 1);
arp_ptr += (arp->ar_hln + arp->ar_pln);
copy_mac_addr(arp_ptr, skb->data);
}
return 0;
default:
return -1;
}
}
/*---------------------------------------------------*/
/* Handle IPX and Apple Talk frame */
/*---------------------------------------------------*/
else if((protocol == __constant_htons(ETH_P_IPX)) ||
(protocol <= __constant_htons(ETH_FRAME_LEN)))
{
struct ipxhdr *ipx = NULL;
struct elapaarp *ea = NULL;
struct ddpehdr *ddp = NULL;
unsigned char *framePtr = skb->data + ETH_HLEN;
if(protocol == __constant_htons(ETH_P_IPX))
{
DEBUG_INFO("NAT25: Protocol=IPX (Ethernet II)\n");
ipx = (struct ipxhdr *)framePtr;
}
else if(protocol <= __constant_htons(ETH_FRAME_LEN))
{
if(RTW_RN16(framePtr) == 0xffff)
{
DEBUG_INFO("NAT25: Protocol=IPX (Ethernet 802.3)\n");
ipx = (struct ipxhdr *)framePtr;
}
else
{
unsigned char ipx_8022_type = 0xE0;
unsigned char snap_8022_type = 0xAA;
if(*framePtr == snap_8022_type)
{
static const u8 ipx_snap_id[5] = {0x00, 0x00, 0x00, 0x81, 0x37}; // IPX SNAP ID
static const u8 aarp_snap_id[5] = {0x00, 0x00, 0x00, 0x80, 0xF3}; // Apple Talk AARP SNAP ID
static const u8 ddp_snap_id[5] = {0x08, 0x00, 0x07, 0x80, 0x9B}; // Apple Talk DDP SNAP ID
framePtr += 3; // eliminate the 802.2 header
if(!memcmp(ipx_snap_id, framePtr, 5))
{
framePtr += 5; // eliminate the SNAP header
DEBUG_INFO("NAT25: Protocol=IPX (Ethernet SNAP)\n");
ipx = (struct ipxhdr *)framePtr;
}
else if(!memcmp(aarp_snap_id, framePtr, 5))
{
framePtr += 5; // eliminate the SNAP header
ea = (struct elapaarp *)framePtr;
}
else if(!memcmp(ddp_snap_id, framePtr, 5))
{
framePtr += 5; // eliminate the SNAP header
ddp = (struct ddpehdr *)framePtr;
}
else
{
DEBUG_WARN("NAT25: Protocol=Ethernet SNAP %02x%02x%02x%02x%02x\n", framePtr[0],
framePtr[1], framePtr[2], framePtr[3], framePtr[4]);
return -1;
}
}
else if(*framePtr == ipx_8022_type)
{
framePtr += 3; // eliminate the 802.2 header
if (RTW_RN16(framePtr) == 0xffff)
{
DEBUG_INFO("NAT25: Protocol=IPX (Ethernet 802.2)\n");
ipx = (struct ipxhdr *)framePtr;
}
else
return -1;
}
else
return -1;
}
}
else
return -1;
/* IPX */
if(ipx != NULL)
{
switch(method)
{
case NAT25_CHECK:
if(mac_addr_equal(skb->data+ETH_ALEN, ipx->ipx_source.node))
{
DEBUG_INFO("NAT25: Check IPX skb_copy\n");
return 0;
}
return -1;
case NAT25_INSERT:
{
DEBUG_INFO("NAT25: Insert IPX, Dest=%08x,%02x%02x%02x%02x%02x%02x,%04x Source=%08x,%02x%02x%02x%02x%02x%02x,%04x\n",
ipx->ipx_dest.net,
ipx->ipx_dest.node[0],
ipx->ipx_dest.node[1],
ipx->ipx_dest.node[2],
ipx->ipx_dest.node[3],
ipx->ipx_dest.node[4],
ipx->ipx_dest.node[5],
ipx->ipx_dest.sock,
ipx->ipx_source.net,
ipx->ipx_source.node[0],
ipx->ipx_source.node[1],
ipx->ipx_source.node[2],
ipx->ipx_source.node[3],
ipx->ipx_source.node[4],
ipx->ipx_source.node[5],
ipx->ipx_source.sock);
if(mac_addr_equal(skb->data+ETH_ALEN, ipx->ipx_source.node))
{
DEBUG_INFO("NAT25: Use IPX Net, and Socket as network addr\n");
__nat25_generate_ipx_network_addr_with_socket(networkAddr, &ipx->ipx_source.net, &ipx->ipx_source.sock);
// change IPX source node addr to wlan STA address
copy_mac_addr(ipx->ipx_source.node, GET_MY_HWADDR(priv));
}
else
{
__nat25_generate_ipx_network_addr_with_node(networkAddr, &ipx->ipx_source.net, ipx->ipx_source.node);
}
__nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
__nat25_db_print(priv);
}
return 0;
case NAT25_LOOKUP:
{
if(mac_addr_equal(GET_MY_HWADDR(priv), ipx->ipx_dest.node))
{
DEBUG_INFO("NAT25: Lookup IPX, Modify Destination IPX Node addr\n");
__nat25_generate_ipx_network_addr_with_socket(networkAddr, &ipx->ipx_dest.net, &ipx->ipx_dest.sock);
__nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
// replace IPX destination node addr with Lookup destination MAC addr
copy_mac_addr(ipx->ipx_dest.node, skb->data);
}
else
{
__nat25_generate_ipx_network_addr_with_node(networkAddr, &ipx->ipx_dest.net, ipx->ipx_dest.node);
__nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
}
}
return 0;
default:
return -1;
}
}
/* AARP */
else if(ea != NULL)
{
/* Sanity check fields. */
if(ea->hw_len != ETH_ALEN || ea->pa_len != AARP_PA_ALEN)
{
DEBUG_WARN("NAT25: Appletalk AARP Sanity check fail!\n");
return -1;
}
switch(method)
{
case NAT25_CHECK:
return 0;
case NAT25_INSERT:
{
// change to AARP source mac address to wlan STA address
copy_mac_addr(ea->hw_src, GET_MY_HWADDR(priv));
DEBUG_INFO("NAT25: Insert AARP, Source=%d,%d Destination=%d,%d\n",
ea->pa_src_net,
ea->pa_src_node,
ea->pa_dst_net,
ea->pa_dst_node);
__nat25_generate_apple_network_addr(networkAddr, &ea->pa_src_net, &ea->pa_src_node);
__nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
__nat25_db_print(priv);
}
return 0;
case NAT25_LOOKUP:
{
DEBUG_INFO("NAT25: Lookup AARP, Source=%d,%d Destination=%d,%d\n",
ea->pa_src_net,
ea->pa_src_node,
ea->pa_dst_net,
ea->pa_dst_node);
__nat25_generate_apple_network_addr(networkAddr, &ea->pa_dst_net, &ea->pa_dst_node);
__nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
// change to AARP destination mac address to Lookup result
copy_mac_addr(ea->hw_dst, skb->data);
}
return 0;
default:
return -1;
}
}
/* DDP */
else if(ddp != NULL)
{
switch(method)
{
case NAT25_CHECK:
return -1;
case NAT25_INSERT:
{
DEBUG_INFO("NAT25: Insert DDP, Source=%d,%d Destination=%d,%d\n",
ddp->deh_snet,
ddp->deh_snode,
ddp->deh_dnet,
ddp->deh_dnode);
__nat25_generate_apple_network_addr(networkAddr, &ddp->deh_snet, &ddp->deh_snode);
__nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
__nat25_db_print(priv);
}
return 0;
case NAT25_LOOKUP:
{
DEBUG_INFO("NAT25: Lookup DDP, Source=%d,%d Destination=%d,%d\n",
ddp->deh_snet,
ddp->deh_snode,
ddp->deh_dnet,
ddp->deh_dnode);
__nat25_generate_apple_network_addr(networkAddr, &ddp->deh_dnet, &ddp->deh_dnode);
__nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
}
return 0;
default:
return -1;
}
}
return -1;
}
/*---------------------------------------------------*/
/* Handle PPPoE frame */
/*---------------------------------------------------*/
else if((protocol == __constant_htons(ETH_P_PPP_DISC)) ||
(protocol == __constant_htons(ETH_P_PPP_SES)))
{
struct pppoe_hdr *ph = (struct pppoe_hdr *)(skb->data + ETH_HLEN);
unsigned short *pMagic;
switch(method)
{
case NAT25_CHECK:
if (ph->sid == 0)
return 0;
return 1;
case NAT25_INSERT:
if(ph->sid == 0) // Discovery phase according to tag
{
if(ph->code == PADI_CODE || ph->code == PADR_CODE)
{
if (priv->ethBrExtInfo.addPPPoETag) {
struct pppoe_tag *tag, *pOldTag;
unsigned char tag_buf[40];
int old_tag_len=0;
tag = (struct pppoe_tag *)tag_buf;
pOldTag = (struct pppoe_tag *)__nat25_find_pppoe_tag(ph, ntohs(PTT_RELAY_SID));
if (pOldTag) { // if SID existed, copy old value and delete it
old_tag_len = ntohs(pOldTag->tag_len);
if (old_tag_len+TAG_HDR_LEN+MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN > sizeof(tag_buf)) {
DEBUG_ERR("SID tag length too long!\n");
return -1;
}
memcpy(tag->tag_data+MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN,
pOldTag->tag_data, old_tag_len);
if (skb_pull_and_merge(skb, (unsigned char *)pOldTag, TAG_HDR_LEN+old_tag_len) < 0) {
DEBUG_ERR("call skb_pull_and_merge() failed in PADI/R packet!\n");
return -1;
}
ph->length = htons(ntohs(ph->length)-TAG_HDR_LEN-old_tag_len);
}
tag->tag_type = PTT_RELAY_SID;
tag->tag_len = htons(MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN+old_tag_len);
// insert the magic_code+client mac in relay tag
pMagic = (unsigned short *)tag->tag_data;
*pMagic = htons(MAGIC_CODE);
copy_mac_addr(tag->tag_data+MAGIC_CODE_LEN, skb->data+ETH_ALEN);
//Add relay tag
if(__nat25_add_pppoe_tag(skb, tag) < 0)
return -1;
DEBUG_INFO("NAT25: Insert PPPoE, forward %s packet\n",
(ph->code == PADI_CODE ? "PADI" : "PADR"));
}
else { // not add relay tag
if (priv->pppoe_connection_in_progress &&
!mac_addr_equal(skb->data+ETH_ALEN, priv->pppoe_addr)) {
DEBUG_ERR("Discard PPPoE packet due to another PPPoE connection is in progress!\n");
return -2;
}
if (priv->pppoe_connection_in_progress == 0)
copy_mac_addr(priv->pppoe_addr, skb->data+ETH_ALEN);
priv->pppoe_connection_in_progress = WAIT_TIME_PPPOE;
}
}
else
return -1;
}
else // session phase
{
DEBUG_INFO("NAT25: Insert PPPoE, insert session packet to %s\n", skb->dev->name);
__nat25_generate_pppoe_network_addr(networkAddr, skb->data, &(ph->sid));
__nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
__nat25_db_print(priv);
if (!priv->ethBrExtInfo.addPPPoETag &&
priv->pppoe_connection_in_progress &&
mac_addr_equal(skb->data+ETH_ALEN, priv->pppoe_addr))
priv->pppoe_connection_in_progress = 0;
}
return 0;
case NAT25_LOOKUP:
if(ph->code == PADO_CODE || ph->code == PADS_CODE)
{
if (priv->ethBrExtInfo.addPPPoETag) {
struct pppoe_tag *tag;
unsigned char *ptr;
unsigned short tagType, tagLen;
int offset=0;
if((ptr = __nat25_find_pppoe_tag(ph, ntohs(PTT_RELAY_SID))) == 0) {
DEBUG_ERR("Fail to find PTT_RELAY_SID in FADO!\n");
return -1;
}
tag = (struct pppoe_tag *)ptr;
tagType = RTW_RB16(&ptr[0]);
tagLen = RTW_RB16(&ptr[2]);
if((tagType != ntohs(PTT_RELAY_SID)) || (tagLen < (MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN))) {
DEBUG_ERR("Invalid PTT_RELAY_SID tag length [%d]!\n", tagLen);
return -1;
}
pMagic = (unsigned short *)tag->tag_data;
if (ntohs(*pMagic) != MAGIC_CODE) {
DEBUG_ERR("Can't find MAGIC_CODE in %s packet!\n",
(ph->code == PADO_CODE ? "PADO" : "PADS"));
return -1;
}
copy_mac_addr(skb->data, tag->tag_data+MAGIC_CODE_LEN);
if (tagLen > MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN)
offset = TAG_HDR_LEN;
if (skb_pull_and_merge(skb, ptr+offset, TAG_HDR_LEN+MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN-offset) < 0) {
DEBUG_ERR("call skb_pull_and_merge() failed in PADO packet!\n");
return -1;
}
ph->length = htons(ntohs(ph->length)-(TAG_HDR_LEN+MAGIC_CODE_LEN+RTL_RELAY_TAG_LEN-offset));
if (offset > 0)
tag->tag_len = htons(tagLen-MAGIC_CODE_LEN-RTL_RELAY_TAG_LEN);
DEBUG_INFO("NAT25: Lookup PPPoE, forward %s Packet from %s\n",
(ph->code == PADO_CODE ? "PADO" : "PADS"), skb->dev->name);
}
else { // not add relay tag
if (!priv->pppoe_connection_in_progress) {
DEBUG_ERR("Discard PPPoE packet due to no connection in progresss!\n");
return -1;
}
copy_mac_addr(skb->data, priv->pppoe_addr);
priv->pppoe_connection_in_progress = WAIT_TIME_PPPOE;
}
}
else {
if(ph->sid != 0)
{
DEBUG_INFO("NAT25: Lookup PPPoE, lookup session packet from %s\n", skb->dev->name);
__nat25_generate_pppoe_network_addr(networkAddr, skb->data+ETH_ALEN, &(ph->sid));
__nat25_db_network_lookup_and_replace(priv, skb, networkAddr);
__nat25_db_print(priv);
}
else
return -1;
}
return 0;
default:
return -1;
}
}
/*---------------------------------------------------*/
/* Handle EAP frame */
/*---------------------------------------------------*/
else if(protocol == __constant_htons(0x888e))
{
switch(method)
{
case NAT25_CHECK:
return -1;
case NAT25_INSERT:
return 0;
case NAT25_LOOKUP:
return 0;
default:
return -1;
}
}
/*---------------------------------------------------*/
/* Handle C-Media proprietary frame */
/*---------------------------------------------------*/
else if((protocol == __constant_htons(0xe2ae)) ||
(protocol == __constant_htons(0xe2af)))
{
switch(method)
{
case NAT25_CHECK:
return -1;
case NAT25_INSERT:
return 0;
case NAT25_LOOKUP:
return 0;
default:
return -1;
}
}
/*---------------------------------------------------*/
/* Handle IPV6 frame */
/*---------------------------------------------------*/
#ifdef CL_IPV6_PASS
else if(protocol == __constant_htons(ETH_P_IPV6))
{
struct ipv6hdr *iph = (struct ipv6hdr *)(skb->data + ETH_HLEN);
if (sizeof(*iph) >= (skb->len - ETH_HLEN))
{
DEBUG_WARN("NAT25: malformed IPv6 packet !\n");
return -1;
}
switch(method)
{
case NAT25_CHECK:
if (skb->data[0] & 1)
return 0;
return -1;
case NAT25_INSERT:
{
DEBUG_INFO("NAT25: Insert IP, SA=%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x,"
" DA=%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x\n",
iph->saddr.s6_addr16[0],iph->saddr.s6_addr16[1],iph->saddr.s6_addr16[2],iph->saddr.s6_addr16[3],
iph->saddr.s6_addr16[4],iph->saddr.s6_addr16[5],iph->saddr.s6_addr16[6],iph->saddr.s6_addr16[7],
iph->daddr.s6_addr16[0],iph->daddr.s6_addr16[1],iph->daddr.s6_addr16[2],iph->daddr.s6_addr16[3],
iph->daddr.s6_addr16[4],iph->daddr.s6_addr16[5],iph->daddr.s6_addr16[6],iph->daddr.s6_addr16[7]);
if (memchr_inv(&iph->saddr, 0, 16)) {
__nat25_generate_ipv6_network_addr(networkAddr, (unsigned int *)&iph->saddr);
__nat25_db_network_insert(priv, skb->data+ETH_ALEN, networkAddr);
__nat25_db_print(priv);
if (iph->nexthdr == IPPROTO_ICMPV6 &&
skb->len > (ETH_HLEN + sizeof(*iph) + 4)) {
if (update_nd_link_layer_addr(skb->data + ETH_HLEN + sizeof(*iph),
skb->len - ETH_HLEN - sizeof(*iph), GET_MY_HWADDR(priv))) {
struct icmp6hdr *hdr = (struct icmp6hdr *)(skb->data + ETH_HLEN + sizeof(*iph));
hdr->icmp6_cksum = 0;
hdr->icmp6_cksum = csum_ipv6_magic(&iph->saddr, &iph->daddr,
iph->payload_len,
IPPROTO_ICMPV6,
csum_partial((__u8 *)hdr, iph->payload_len, 0));
}
}
}
}
return 0;
case NAT25_LOOKUP:
DEBUG_INFO("NAT25: Lookup IP, SA=%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x,"
" DA=%4x:%4x:%4x:%4x:%4x:%4x:%4x:%4x\n",
iph->saddr.s6_addr16[0],iph->saddr.s6_addr16[1],iph->saddr.s6_addr16[2],iph->saddr.s6_addr16[3],
iph->saddr.s6_addr16[4],iph->saddr.s6_addr16[5],iph->saddr.s6_addr16[6],iph->saddr.s6_addr16[7],
iph->daddr.s6_addr16[0],iph->daddr.s6_addr16[1],iph->daddr.s6_addr16[2],iph->daddr.s6_addr16[3],
iph->daddr.s6_addr16[4],iph->daddr.s6_addr16[5],iph->daddr.s6_addr16[6],iph->daddr.s6_addr16[7]);
__nat25_generate_ipv6_network_addr(networkAddr, (unsigned int *)&iph->daddr);
if (!__nat25_db_network_lookup_and_replace(priv, skb, networkAddr)) {
#ifdef SUPPORT_RX_UNI2MCAST
if (iph->daddr.s6_addr[0] == 0xff)
convert_ipv6_mac_to_mc(skb);
#endif
}
return 0;
default:
return -1;
}
}
#endif // CL_IPV6_PASS
return -1;
}
int rtw_mlcst2unicst(_adapter *padapter, struct sk_buff *skb)
{
struct sta_priv *pstapriv = &padapter->stapriv;
struct xmit_priv *pxmitpriv = &padapter->xmitpriv;
_irqL irqL;
_list *phead, *plist;
struct sk_buff *newskb;
struct sta_info *psta = NULL;
u8 chk_alive_num = 0;
char chk_alive_list[NUM_STA];
int i;
s32 res;
_enter_critical_bh(&pstapriv->asoc_list_lock, &irqL);
phead = &pstapriv->asoc_list;
plist = get_next(phead);
//free sta asoc_queue
while (!(rtw_end_of_queue_search(phead, plist)) ) {
int stainfo_offset;
psta = LIST_CONTAINOR(plist, struct sta_info, asoc_list);
plist = get_next(plist);
stainfo_offset = rtw_stainfo_offset(pstapriv, psta);
if (stainfo_offset_valid(stainfo_offset)) {
chk_alive_list[chk_alive_num++] = stainfo_offset;
}
}
_exit_critical_bh(&pstapriv->asoc_list_lock, &irqL);
for (i = 0; i < chk_alive_num; i++) {
psta = rtw_get_stainfo_by_offset(pstapriv, chk_alive_list[i]);
if(!(psta->state &_FW_LINKED))
continue;
/* avoid come from STA1 and send back STA1 */
if (mac_addr_equal(psta->hwaddr, &skb->data[6])
|| is_zero_mac_addr(psta->hwaddr)
|| is_broadcast_mac_addr(psta->hwaddr)
)
continue;
newskb = rtw_skb_copy(skb);
if (newskb) {
copy_mac_addr(newskb->data, psta->hwaddr);
res = rtw_xmit(padapter, &newskb);
if (res < 0) {
DBG_871X("%s()-%d: rtw_xmit() return error!\n", __FUNCTION__, __LINE__);
pxmitpriv->tx_drop++;
rtw_skb_free(newskb);
} else
pxmitpriv->tx_pkts++;
} else {
DBG_871X("%s-%d: rtw_skb_copy() failed!\n", __FUNCTION__, __LINE__);
pxmitpriv->tx_drop++;
//rtw_skb_free(skb);
return _FALSE; // Caller shall tx this multicast frame via normal way.
}
}
rtw_skb_free(skb);
return _TRUE;
}