//---------------------------------------------------------------------------
int nasrg_netlink_send(unsigned char *data_buffer, unsigned int data_length, int destination)
{
//---------------------------------------------------------------------------
struct sk_buff *nl_skb;
struct nlmsghdr *nlh;
int status;
// Start debug information
#ifdef NETLINK_DEBUG
printk("nasrg_netlink_send - begin \n");
#endif
if (!data_buffer) {
printk("nasrg_netlink_send - ERROR - input parameter data is NULL \n");
return(0);
}
if (!nas_nl_sk || !nas_rrcnl_sk) {
printk("nasrg_netlink_send - ERROR - socket is NULL\n");
return(0);
}
// End debug information
nl_skb = alloc_skb(NLMSG_SPACE(data_length),GFP_ATOMIC);
if (!nl_skb) {
printk("nasrg_netlink_send - ERROR - could not allocate skbuffer\n");
return(0);
}
nlh = (struct nlmsghdr *)nl_skb->data;
// printk("nasrg_netlink_send Sending %d bytes (%d)\n",data_length,NLMSG_SPACE(data_length));
skb_put(nl_skb, NLMSG_SPACE(data_length));
memcpy(NLMSG_DATA(nlh), data_buffer, data_length);
nlh->nlmsg_len = NLMSG_SPACE(data_length);
nlh->nlmsg_pid = 0; /* from kernel */
NETLINK_CB(nl_skb).pid = 0;
// destination 0 = PDCP, 1 = RRC
if (destination== NASNL_DEST_PDCP) {
#ifdef NETLINK_DEBUG
printk("nasrg_netlink_send - Sending to PDCP - nl_skb %p, nl_sk %p, nlh %p, nlh->nlmsg_len %d\n", nl_skb, nas_nl_sk, nlh, nlh->nlmsg_len);
#ifdef NAS_DEBUG_SEND_DETAIL
nasrg_TOOL_print_buffer(NLMSG_DATA(nlh),48);
#endif
#endif //DEBUG_NETLINK
status = netlink_unicast(nas_nl_sk, nl_skb, NL_DEST_PID, MSG_DONTWAIT);
} else {
#ifdef NAS_DEBUG_RRCNL
printk("nasrg_netlink_send - Sending to RRC - nl_skb %p, nas_rrcnl_sk %p, nlh %p, nlh->nlmsg_len %d\n", nl_skb, nas_rrcnl_sk, nlh, nlh->nlmsg_len);
nasrg_TOOL_print_buffer(NLMSG_DATA(nlh),data_length);
#endif //NAS_DEBUG_RRCNL
status = netlink_unicast(nas_rrcnl_sk, nl_skb, NL_DEST_RRC_PID, MSG_DONTWAIT);
}
if (status < 0) {
printk("nasrg_netlink_send - SEND status is %d\n",status);
return(0);
} else {
#ifdef NETLINK_DEBUG
printk("nasrg_netlink_send - SEND status is %d, data_length %d\n",status, data_length);
#endif
return data_length;
}
}
//---------------------------------------------------------------------------
// Search the sending function for IP Packet
void nasrg_CLASS_send(struct sk_buff *skb)
{
//---------------------------------------------------------------------------
struct classifier_entity *pclassifier, *sp;
uint8_t *protocolh = NULL;
uint8_t version;
uint8_t protocol, dscp;
uint16_t classref;
struct cx_entity *cx;
unsigned int i;
#ifdef NAS_DEBUG_CLASS
char sfct[10], sprotocol[10];
#endif
struct net_device *dev = gdev;
unsigned char cx_index,no_connection;
int addr_type;
int mbms_ix;
struct in6_addr masked6_addr;
struct in_addr masked_addr;
// RARP vars
struct arphdr *rarp;
unsigned char *rarp_ptr;
/* s for "source", t for "target" */
__be32 sip, tip;
unsigned char *sha, *tha;
#ifdef NAS_DEBUG_CLASS
printk("nasrg_CLASS_send: begin -\n");
#endif
if (skb==NULL) {
printk("nasrg_CLASS_send - input parameter skb is NULL \n");
return;
}
//***
#ifdef NAS_DEBUG_SEND
printk("nasrg_CLASS_send - Received IP packet to transmit, length %d\n", skb->len);
#endif
#ifdef NAS_DEBUG_SEND_DETAIL
if ((skb->data) != NULL) {
if (skb->len<150)
nasrg_TOOL_print_buffer(skb->data,skb->len);
else
nasrg_TOOL_print_buffer(skb->data,150);
}
#endif
//***
// find all connections related to socket
cx_index = 0;
no_connection = 1;
cx = NULL;
#ifdef NAS_DEBUG_CLASS
printk("nasrg_CLASS_send: [before switch on IP protocol version] \n");
#endif
// Get mobile connexion entity, protocol and dscp from IP packet
switch (ntohs(skb->protocol)) {
case ETH_P_IPV6:
#ifdef NAS_DEBUG_CLASS_DETAIL
printk("nasrg_CLASS_send : skb->protocol : IPv6 \n");
#endif
version = NAS_VERSION_6;
addr_type = NAS_IPV6_ADDR_UNKNOWN;
protocolh = nasrg_TOOL_get_protocol6(ipv6_hdr(skb), &protocol);
dscp = nasrg_TOOL_get_dscp6 (ipv6_hdr(skb));
cx = nasrg_CLASS_cx6 (skb, dscp, &addr_type, &cx_index, &mbms_ix);
#ifdef NAS_DEBUG_CLASS_DETAIL
printk("nasrg_CLASS_send - ETH_P_IPV6 skb %p dscp %d gpriv %p cx_index %p \n",skb, dscp, gpriv, &cx_index);
#endif
// find in default DSCP a valid classification
if (cx == NULL) {
switch (addr_type) {
case NAS_IPV6_ADDR_MC_SIGNALLING:
case NAS_IPV6_ADDR_UNICAST:
#ifdef NAS_DEBUG_CLASS_DETAIL
printk("nasrg_CLASS_send - case NAS_IPV6_ADDR_MC_SIGNALLING | NAS_IPV6_ADDR_UNICAST\n");
#endif //NAS_DEBUG_CLASS
for (i=0; i<NAS_CX_MAX; i++) {
pclassifier=(&gpriv->cx[i])->sclassifier[NAS_DSCP_DEFAULT];
while (pclassifier!=NULL) {
if ((pclassifier->version == NAS_VERSION_6) || (pclassifier->version == NAS_VERSION_DEFAULT)) {
// ok found default classifier for this packet
nasrg_create_mask_ipv6_addr(&masked6_addr, pclassifier->dplen);
// Modified MW to let everything go (pb with signalling)
masked6_addr.s6_addr32[0] = 0x00000000;
masked6_addr.s6_addr32[1] = 0x00000000;
#ifdef NAS_DEBUG_CLASS_DETAIL
printk("nasrg_CLASS_send - cx %d : DSCP NAS_DSCP_DEFAULT %X:%X:%X:%X:%X:%X:%X:%X\n",i, NIP6ADDR(&(pclassifier->daddr.ipv6)));
#endif //NAS_DEBUG_CLASS
if (IN6_ARE_ADDR_MASKED_EQUAL(&pclassifier->daddr.ipv6, &ipv6_hdr(skb)->daddr, &masked6_addr)) {
// then force dscp
cx = &gpriv->cx[i];
#ifdef NAS_DEBUG_CLASS
printk("nasrg_CLASS_send - ETH_P_IPV6 FOUND NAS_DSCP_DEFAULT with IN6_ARE_ADDR_MASKED_EQUAL(%d bits)\n",pclassifier->dplen);
#endif
dscp = NAS_DSCP_DEFAULT;
break;
} else {
if(IN6_IS_ADDR_UNSPECIFIED(&pclassifier->daddr.ipv6)) {
cx = &gpriv->cx[i];
#ifdef NAS_DEBUG_CLASS
printk("nasrg_CLASS_send - ETH_P_IPV6 FOUND NAS_DSCP_DEFAULT with IN6_IS_ADDR_UNSPECIFIED\n");
#endif
dscp = NAS_DSCP_DEFAULT;
break;
}
}
}
pclassifier = pclassifier->next;
}
}
break;
// MBMS is broken!!!! To be updated (these values will be over-ridden afterwards
case NAS_IPV6_ADDR_MC_MBMS:
#ifdef NAS_DEBUG_CLASS
printk("nasrg_CLASS_send - case NAS_IPV6_ADDR_MC_MBMS\n");
#endif //NAS_DEBUG_CLASS
pclassifier = gpriv->mbmsclassifier[mbms_ix];
printk("nasrg_CLASS_send: MBMS is broken!!!!\n\n\n");
sp = gpriv->mbmsclassifier[mbms_ix];
if (sp!= NULL) {
classref=sp->classref;
#ifdef NAS_DEBUG_CLASS_DETAIL
printk("nasrg_CLASS_send: classifier found for multicast service %d \n", mbms_ix);
#endif
} else {
printk("nasrg_CLASS_send: No corresponding multicast bearer, so the message is dropped\n");
return;
}
break;
// should have found a valid classification rule
case NAS_IPV6_ADDR_UNKNOWN:
default:
printk("nasrg_CLASS_send: No corresponding address type\n");
}
}
break;
case ETH_P_ARP:
#ifdef NAS_DEBUG_CLASS
printk("nasrg_CLASS_send : skb->protocol : ARP \n");
#endif
version = NAS_VERSION_4;
addr_type = NAS_IPV4_ADDR_BROADCAST;
dscp = 0;
cx = NULL;
// Basic sanity checks can be done without the lock
rarp = (struct arphdr *)skb_network_header(skb);
if (rarp) {
if (rarp->ar_hln != dev->addr_len || dev->type != ntohs(rarp->ar_hrd)) {
printk("nasrg_CLASS_send: ARP PACKET WRONG ADDR LEN or WRONG ARP HEADER TYPE\n");
break;
}
} else {
printk("nasrg_CLASS_send: ARP HEADER POINTER IS NULL\n");
break;
}
// If it's not Ethernet, delete it.
if (rarp->ar_pro != htons(ETH_P_IP)) {
printk("nasrg_CLASS_send: ARP PACKET PROTOCOL IS NOT ETHERNET\n");
break;
}
rarp_ptr = (unsigned char *) (rarp + 1);
sha = rarp_ptr;
rarp_ptr += dev->addr_len;
memcpy(&sip, rarp_ptr, 4);
rarp_ptr += 4;
tha = rarp_ptr;
rarp_ptr += dev->addr_len;
memcpy(&tip, rarp_ptr, 4);
#ifdef NAS_DEBUG_CLASS
printk("nasrg_CLASS_send: ARP DEST IP transport IP = %d.%d.%d.%d\n",NIPADDR(tip));
#endif
for (i=0; i<NAS_CX_MAX; i++) {
pclassifier=(&gpriv->cx[i])->sclassifier[NAS_DSCP_DEFAULT];
while (pclassifier!=NULL) {
if ((pclassifier->version == NAS_VERSION_4) || (pclassifier->version == NAS_VERSION_DEFAULT)) {
// ok found default classifier for this packet
nasrg_create_mask_ipv4_addr(&masked_addr, pclassifier->dplen);
#ifdef NAS_DEBUG_CLASS
printk("nasrg_CLASS_send: MASK = %d.%d.%d.%d\n",NIPADDR(masked_addr.s_addr));
#endif
//
if (IN_ARE_ADDR_MASKED_EQUAL(&pclassifier->daddr.ipv4, &tip, &masked_addr.s_addr)) {
// then force dscp
cx = &gpriv->cx[i];
#ifdef NAS_DEBUG_CLASS
printk("nasrg_CLASS_send: ETH_P_ARP FOUND NAS_DSCP_DEFAULT with IN_ARE_ADDR_MASKED_EQUAL(%d bits)\n", pclassifier->dplen);
#endif
dscp = NAS_DSCP_DEFAULT;
break;
} else {
if (INADDR_ANY == pclassifier->daddr.ipv4) {
cx = &gpriv->cx[i];
#ifdef NAS_DEBUG_CLASS
printk("nasrg_CLASS_send: ETH_P_ARP FOUND NAS_DSCP_DEFAULT with INADDR_ANY\n");
#endif
dscp = NAS_DSCP_DEFAULT;
break;
}
}
}
pclassifier = pclassifier->next;
}
}
break;
case ETH_P_IP:
#ifdef NAS_DEBUG_CLASS_DETAIL
printk("nasrg_CLASS_send : skb->protocol : IPv4 \n");
#endif
version = NAS_VERSION_4;
addr_type = NAS_IPV4_ADDR_UNKNOWN;
dscp = nasrg_TOOL_get_dscp4((struct iphdr *)(skb_network_header(skb)));
cx = nasrg_CLASS_cx4(skb, dscp, &addr_type, &cx_index);
protocolh = nasrg_TOOL_get_protocol4((struct iphdr *)(skb_network_header(skb)), &protocol);
// find in default DSCP a valid classification
if (cx == NULL) {
switch (addr_type) {
case NAS_IPV4_ADDR_MC_SIGNALLING:
case NAS_IPV4_ADDR_UNICAST:
case NAS_IPV4_ADDR_BROADCAST:
for (i=0; i<NAS_CX_MAX; i++) {
pclassifier=(&gpriv->cx[i])->sclassifier[NAS_DSCP_DEFAULT];
while (pclassifier != NULL) {
if ((pclassifier->version == NAS_VERSION_4) || (pclassifier->version == NAS_VERSION_DEFAULT)) {
// ok found default classifier for this packet
nasrg_create_mask_ipv4_addr(&masked_addr, pclassifier->dplen);
#ifdef NAS_DEBUG_CLASS_DETAIL
printk("nasrg_CLASS_send : MASK = %d.%d.%d.%d\n", NIPADDR(masked_addr.s_addr));
#endif
if (IN_ARE_ADDR_MASKED_EQUAL(&pclassifier->daddr.ipv4, &ip_hdr(skb)->daddr, &masked_addr.s_addr)) {
// then force dscp
cx = &gpriv->cx[i];
#ifdef NAS_DEBUG_CLASS
printk("nasrg_CLASS_send : ETH_P_IP FOUND NAS_DSCP_DEFAULT with IN_ARE_ADDR_MASKED_EQUAL(%d bits)\n",pclassifier->dplen);
#endif
dscp = NAS_DSCP_DEFAULT;
break;
} else {
if(INADDR_ANY == pclassifier->daddr.ipv4) {
cx = &gpriv->cx[i];
#ifdef NAS_DEBUG_CLASS
printk("nasrg_CLASS_send : ETH_P_IP FOUND NAS_DSCP_DEFAULT with INADDR_ANY\n");
#endif
dscp = NAS_DSCP_DEFAULT;
break;
}
}
}
pclassifier = pclassifier->next;
}
}
break;
// should have found a valid classification rule
case NAS_IPV4_ADDR_UNKNOWN:
default:
printk("nasrg_CLASS_send: No corresponding address type\n");
}
}
#ifdef NAS_DEBUG_CLASS
if (cx)
printk("nasrg_CLASS_send: ETH_P_IP Received IPv4 packet (%02X), dscp = %d, cx = %d\n",ntohs(skb->protocol),dscp,cx->lcr);
else
printk("nasrg_CLASS_send: ETH_P_IP Received IPv4 packet (%02X), dscp = %d, No valid connection\n",ntohs(skb->protocol),dscp);
#endif
break;
default:
printk("nasrg_CLASS_send: Unknown IP version protocol\n");
version = 0;
return;
}
#ifdef NAS_DEBUG_CLASS_DETAIL
printk("nasrg_CLASS_send: [before if (cx != NULL)]\n");
#endif
//Next lines bypass classifiers to test the netlink socket
//#define DEBUG_NETLINKRG_TEST
#ifdef DEBUG_NETLINKRG_TEST
nasrg_COMMON_QOS_send_test_netlink(skb);
return;
#endif
// If a valid connection for the DSCP/EXP with destination address
// is found scan all protocol-based classification rules
if (cx != NULL) {
classref = 0;
sp = NULL;
if (cx->state!=NAS_CX_DCH) {
#ifdef NAS_DEBUG_CLASS
printk("nasrg_CLASS_send: UE not connected, in state %d. Packet is dropped\n",cx->state);
#endif
return;
}
if (addr_type==NAS_IPV6_ADDR_MC_MBMS) {
sp = gpriv->mbmsclassifier[mbms_ix];
if (sp!= NULL) {
classref=sp->classref;
#ifdef NAS_DEBUG_CLASS
printk("nasrg_CLASS_send: classifier found for multicast index %d, service %d\n", mbms_ix, gpriv->mbms_rb[mbms_ix].cnxid);
#endif
} else {
// Temp MEDIEVAL : use default classifier
sp = cx->sclassifier[NAS_DSCP_DEFAULT];
if (sp!= NULL) {
classref=sp->classref;
#ifdef NAS_DEBUG_CLASS
printk("nasrg_CLASS_send: classifier for multicast service %d replaced by default %d\n", mbms_ix, classref);
#endif
#ifdef NAS_AUTO_MBMS
nasrg_ASCTL_start_default_mbms_service();
#endif
} else {
printk("nasrg_CLASS_send: No corresponding multicast bearer, so the message is dropped\n");
return;
}
}
} else {
#ifdef NAS_DEBUG_CLASS_DETAIL
printk("nasrg_CLASS_send: DSCP %d version %d: looking for classifier entry\n",dscp, version);
#endif
for (pclassifier=cx->sclassifier[dscp]; pclassifier!=NULL; pclassifier=pclassifier->next) {
#ifdef NAS_DEBUG_CLASS_DETAIL
printk("nasrg_CLASS_send: DSCP %d p->classref=%d,p->protocol=%d,p->version=%d\n",dscp,pclassifier->classref,pclassifier->protocol,pclassifier->version);
#endif
// normal rule checks that network protocol version matches
if ((pclassifier->version == version) || (pclassifier->version == NAS_VERSION_DEFAULT)) {
//printk("nasrg_CLASS_send: IP version are equals\n");
sp=pclassifier;
classref=sp->classref;
#ifdef NAS_DEBUG_CLASS_DETAIL
printk("nasrg_CLASS_send: classifier found for dscp %u \n", dscp);
#endif
break;
}
}
}
if (sp!=NULL) {
#ifdef NAS_DEBUG_CLASS
//char sfct[10], sprotocol[10];
// classifier entity found. Print its parameters
if (sp->fct==nasrg_COMMON_QOS_send)
strcpy(sfct, "data xfer");
if (sp->fct==nasrg_CTL_send)
strcpy(sfct, "iocontrol");
if (sp->fct==nasrg_COMMON_del_send)
strcpy(sfct, "delete");
if (sp->fct==nasrg_ASCTL_DC_send_sig_data_request)
strcpy(sfct, "DC-SAP");
switch(protocol) {
case NAS_PROTOCOL_UDP:
strcpy(sprotocol, "udp");
printk("udp packet\n");
break;
case NAS_PROTOCOL_TCP:
strcpy(sprotocol, "tcp");
printk("tcp packet\n");
break;
case NAS_PROTOCOL_ICMP4:
strcpy(sprotocol, "icmp4");
printk("icmp4 packet\n");
break;
case NAS_PROTOCOL_ICMP6:
strcpy(sprotocol, "icmp6");
nasrg_TOOL_pk_icmp6((struct icmp6hdr*)protocolh);
break;
default:
strcpy(sprotocol, "other L4");
break;
}
printk("nasrg_CLASS_send: (dscp %u, %s) received, (classref %u, fct %s, drb_id %u) classifier rule\n",
dscp, sprotocol, sp->classref, sfct, sp->rab_id);
#endif
//forward packet to the correct entity
if (sp->fct!=NULL) {
sp->fct(skb, cx, sp);
} else {
printk("\n\nnasrg_CLASS_send: ERROR : CLASSIFIER FUNCTION IS NULL\n\n");
}
no_connection = 0;
// end : if classifier entry match found
} else {
printk("nasrg_CLASS_send: no corresponding item in the classifier list, so the message is dropped\n");
printk("nasrg_CLASS_send: packet parameters: dscp %u, %s\n", dscp, sprotocol);
nasrg_COMMON_del_send(skb, cx, NULL); // Note MW: LG has commented this line. Why?
}
} // if connection found
#ifdef NAS_DEBUG_CLASS
if (no_connection == 1) {
printk("nasrg_CLASS_send: no corresponding connection, so the message is dropped\n");
}
#endif
#ifdef NAS_DEBUG_CLASS_DETAIL
printk("nasrg_CLASS_send: end\n");
#endif
}