drivers/media/mdtv/smsnet.c

   1 #include <linux/module.h>
   2 #include <linux/init.h>
   3 #include <linux/netdevice.h>    /* struct device, and other headers */
   4 #include <linux/etherdevice.h>  /* eth_type_trans */
   5 #include <linux/ip.h>           /* struct iphdr */
   6 #include <linux/ipv6.h>         /* struct ipv6hdr */
   7 #include <linux/in.h>
   8
   9 #include "smskdefs.h" // page, scatterlist, kmutex
  10 #include "smscoreapi.h"
  11 #include "smstypes.h"
  12
  13 #define IPV4VERSION                     0x40
  14 #define IPV6VERSION                     0x60
  15 #define GETIPVERSION(_x_)       ((_x_) & 0xf0)
  16
  17 typedef struct _smsnet_client
  18 {
  19         struct list_head entry;
  20
  21         smscore_device_t *coredev;
  22         smscore_client_t *smsclient;
  23
  24         int                             packet_length, splitpacket_length;
  25         int                             header_length, splitheader_length;
  26         u8                              splitpacket[ETH_DATA_LEN];
  27 } smsnet_client_t;
  28
  29 struct list_head g_smsnet_clients;
  30 kmutex_t g_smsnet_clientslock;
  31
  32 struct net_device *g_smsnet_device = NULL;
  33 struct net_device_stats g_smsnet_stats;
  34
  35 int     g_smsnet_inuse = 0;
  36
  37 void smsnet_send_packet(u8* buffer, int length)
  38 {
  39         u8 *eth;
  40         struct sk_buff *skb = dev_alloc_skb(length + ETH_HLEN + NET_IP_ALIGN);
  41
  42         if (!skb)
  43         {
  44                 g_smsnet_stats.rx_dropped++;
  45                 return;
  46         }
  47
  48         skb_reserve(skb, NET_IP_ALIGN);
  49
  50         eth = (u8 *) skb_put(skb, length + ETH_HLEN);
  51         memcpy(eth + ETH_HLEN, buffer, length);
  52
  53         eth[6] = 0;
  54         eth[7] = 1;
  55         eth[8] = 1;
  56         eth[9] = 3;
  57         eth[10] = 4;
  58         eth[11] = 5;
  59
  60         if (GETIPVERSION(*buffer) == IPV4VERSION)
  61         {
  62                 eth[0] = 1;
  63                 eth[1] = 0;
  64                 eth[2] = 0x5e;
  65                 eth[3] = buffer[17] & 0x7f;
  66                 eth[4] = buffer[18];
  67                 eth[5] = buffer[19];
  68
  69                 eth[12] = 0x08;
  70                 eth[13] = 0x00;
  71         }
  72         else
  73         {
  74                 // todo: ip6 mcast address
  75
  76                 eth[12] = 0x86;
  77                 eth[13] = 0xdd;
  78         }
  79
  80         skb->dev = g_smsnet_device;
  81         skb->protocol = eth_type_trans(skb, g_smsnet_device);
  82         skb->ip_summed = CHECKSUM_UNNECESSARY;
  83
  84         g_smsnet_stats.rx_packets ++;
  85         g_smsnet_stats.rx_bytes += skb->len;
  86
  87         netif_rx(skb);
  88 }
  89
  90 int check_header(smsnet_client_t* client, u8* buffer)
  91 {
  92         struct iphdr *ip4_hdr;
  93         struct ipv6hdr *ip6_hdr;
  94         struct udphdr *udp_hdr;
  95         u16 csum;
  96
  97         // check if packet header is valid and it is a UDP
  98         if (GETIPVERSION(*buffer) == IPV4VERSION)
  99         {
 100                 ip4_hdr = (struct iphdr*) buffer;
 101                 csum = ip4_hdr->check;
 102
 103                 ip4_hdr->check = 0;
 104
 105                 // check header checksum for IPv4 packets
 106                 if(ip4_hdr->protocol != IPPROTO_UDP || csum != ip_fast_csum(buffer, ip4_hdr->ihl))
 107                 {
 108                         ip4_hdr->check = csum;
 109                         return 0;
 110                 }
 111
 112                 ip4_hdr->check = csum;
 113                 client->packet_length = ntohs(ip4_hdr->tot_len);
 114         }
 115         else
 116         {
 117                 ip6_hdr = (struct ipv6hdr *) buffer;
 118                 udp_hdr = (struct udphdr *)(ip6_hdr + 1);
 119
 120                 if ((ip6_hdr->nexthdr != IPPROTO_UDP) ||
 121                         (ip6_hdr->payload_len != udp_hdr->len))
 122                 {
 123                         return 0;
 124                 }
 125
 126                 client->packet_length = ntohs(ip6_hdr->payload_len) + sizeof(struct ipv6hdr);
 127         }
 128
 129         // check for abnormal packet length
 130         if (client->packet_length > ETH_DATA_LEN)
 131                 return 0;
 132
 133         return 1;
 134 }
 135
 136 int smsnet_onresponse(void *context, smscore_buffer_t *cb)
 137 {
 138         smsnet_client_t *client = (smsnet_client_t *) context;
 139         int                             length, rest;
 140         u8                              ip_ver, *buffer;
 141
 142         buffer = ((u8*) cb->p) + cb->offset + sizeof(SmsMsgHdr_ST);
 143         length = cb->size - sizeof(SmsMsgHdr_ST);
 144
 145         if (client->splitheader_length)
 146         {
 147                 // how much data is missing ?
 148                 rest = client->header_length - client->splitheader_length;
 149
 150                 // do we have enough in this buffer ?
 151                 rest = min(rest, length);
 152
 153                 memcpy(&client->splitpacket[client->splitheader_length], buffer, rest);
 154
 155                 client->splitheader_length += rest;
 156
 157                 if (client->splitheader_length != client->header_length)
 158                         goto exit;
 159
 160                 if (check_header(client, client->splitpacket))
 161                 {
 162                         buffer += rest;
 163                         length -= rest;
 164
 165                         client->splitpacket_length = client->header_length;
 166                 }
 167
 168                 client->splitheader_length = 0;
 169         }
 170
 171         if (client->splitpacket_length)
 172         {
 173                 // how much data is missing ?
 174                 rest = client->packet_length - client->splitpacket_length;
 175
 176                 // do we have enough in this buffer ?
 177                 rest = min(rest, length);
 178
 179                 memcpy(&client->splitpacket[client->splitpacket_length], buffer, rest);
 180
 181                 client->splitpacket_length += rest;
 182
 183                 if (client->splitpacket_length != client->packet_length)
 184                         goto exit;
 185
 186                 client->splitpacket_length = 0;
 187
 188                 smsnet_send_packet(client->splitpacket, client->packet_length);
 189
 190                 buffer += rest;
 191                 length -= rest;
 192         }
 193
 194         while (length > 0)
 195         {
 196                 ip_ver = GETIPVERSION(*buffer);
 197                 while (length && (ip_ver != IPV4VERSION) && (ip_ver != IPV6VERSION))
 198                 {
 199                         buffer++;
 200                         length--;
 201                         ip_ver = GETIPVERSION(*buffer);
 202                 }
 203
 204                 // No more data in section
 205                 if (!length)
 206                         break;
 207
 208                 // Set the header length at start of packet according to the version
 209                 // no problem with the IP header cast, since we have at least 1 byte (we use only the first byte)
 210                 client->header_length = (ip_ver == IPV4VERSION) ? (((struct iphdr *) buffer)->ihl * 4) : (sizeof(struct ipv6hdr) + sizeof(struct udphdr));
 211
 212                 // Check that Header length is at least 20 (min IPv4 length)
 213                 if (client->header_length < 20)
 214                 {
 215                         length--;
 216                         buffer++;
 217                         continue;
 218                 }
 219
 220                 // check split header case
 221                 if (client->header_length > length)
 222                 {
 223                         memcpy(client->splitpacket, buffer, length);
 224                         client->splitheader_length = length;
 225                         break;
 226                 }
 227
 228                 if (check_header(client, buffer))
 229                 {
 230                         // check split packet case
 231                         if (client->packet_length > length)
 232                         {
 233                                 memcpy(client->splitpacket, buffer, length);
 234                                 client->splitpacket_length = length;
 235                                 break;
 236                         }
 237                 }
 238                 else
 239                 {
 240                         length --;
 241                         buffer ++;
 242                         continue;
 243                 }
 244
 245                 smsnet_send_packet(buffer, client->packet_length);
 246
 247                 buffer += client->packet_length;
 248                 length -= client->packet_length;
 249         }
 250
 251 exit:
 252         smscore_putbuffer(client->coredev, cb);
 253
 254         return 0;
 255 }
 256
 257 void smsnet_unregister_client(smsnet_client_t* client)
 258 {
 259         // must be called under clientslock
 260
 261         list_del(&client->entry);
 262
 263         smscore_unregister_client(client->smsclient);
 264         kfree(client);
 265 }
 266
 267 void smsnet_onremove(void *context)
 268 {
 269         kmutex_lock(&g_smsnet_clientslock);
 270
 271         smsnet_unregister_client((smsnet_client_t*) context);
 272
 273         kmutex_unlock(&g_smsnet_clientslock);
 274 }
 275
 276 int smsnet_hotplug(smscore_device_t *coredev, struct device* device, int arrival)
 277 {
 278         smsclient_params_t params;
 279         smsnet_client_t* client;
 280         int rc;
 281
 282         // device removal handled by onremove callback
 283         if (!arrival)
 284                 return 0;
 285
 286         client = kzalloc(sizeof(smsnet_client_t), GFP_KERNEL);
 287         if (!client)
 288         {
 289                 printk(KERN_INFO "%s kmalloc() failed\n", __FUNCTION__);
 290                 return -ENOMEM;
 291         }
 292
 293         params.initial_id = 0;
 294         params.data_type = MSG_SMS_DATA_MSG;
 295         params.onresponse_handler = smsnet_onresponse;
 296         params.onremove_handler = smsnet_onremove;
 297         params.context = client;
 298
 299         rc = smscore_register_client(coredev, &params, &client->smsclient);
 300         if (rc < 0)
 301         {
 302                 printk(KERN_INFO "%s smscore_register_client() failed %d\n", __FUNCTION__, rc);
 303                 kfree(client);
 304                 return rc;
 305         }
 306
 307         client->coredev = coredev;
 308
 309         kmutex_lock(&g_smsnet_clientslock);
 310
 311         list_add(&client->entry, &g_smsnet_clients);
 312
 313         kmutex_unlock(&g_smsnet_clientslock);
 314
 315         printk(KERN_INFO "%s success\n", __FUNCTION__);
 316
 317         return 0;
 318 }
 319
 320 static int smsnet_open(struct net_device *dev)
 321 {
 322         g_smsnet_inuse ++;
 323
 324         netif_start_queue(dev);
 325
 326         printk(KERN_INFO "%s, %d\n", __FUNCTION__, g_smsnet_inuse);
 327
 328         return 0;
 329 }
 330
 331 static int smsnet_stop(struct net_device *dev)
 332 {
 333         netif_stop_queue(dev);
 334
 335         g_smsnet_inuse --;
 336
 337         printk(KERN_INFO "%s, %d\n", __FUNCTION__, g_smsnet_inuse);
 338
 339         return 0;
 340 }
 341
 342 static int smsnet_hard_start_xmit(struct sk_buff *skb, struct net_device *dev)
 343 {
 344         printk(KERN_INFO "%s\n", __FUNCTION__);
 345
 346         dev_kfree_skb(skb);
 347         return 0;
 348 }
 349
 350 static struct net_device_stats * smsnet_get_stats(struct net_device *dev)
 351 {
 352         return &g_smsnet_stats;
 353 }
 354
 355 static void smsnet_set_multicast_list(struct net_device *dev)
 356 {
 357         printk(KERN_INFO "%s %d\n", __FUNCTION__, dev->mc_count);
 358         if (dev->mc_count)
 359         {
 360                 struct dev_mc_list      *p;
 361
 362                 for (p = dev->mc_list; p; p = p->next)
 363                         printk(KERN_INFO "%s %d %02x %02x %02x %02x %02x %02x %02x %02x\n", __FUNCTION__, p->dmi_addrlen,
 364                                 p->dmi_addr[0], p->dmi_addr[1], p->dmi_addr[2], p->dmi_addr[3],
 365                                 p->dmi_addr[4], p->dmi_addr[5], p->dmi_addr[6], p->dmi_addr[7]
 366                         );
 367         }
 368 }
 369
 370 static void smsnet_setup_device(struct net_device *dev)
 371 {
 372         ether_setup(dev);
 373
 374         dev->open                               = smsnet_open;
 375         dev->stop                               = smsnet_stop;
 376         dev->hard_start_xmit    = smsnet_hard_start_xmit;
 377         dev->get_stats                  = smsnet_get_stats;
 378         dev->set_multicast_list = smsnet_set_multicast_list;
 379
 380         dev->mc_count = 0;
 381         dev->hard_header_cache  = NULL;
 382
 383         memcpy(dev->dev_addr, "\0SIANO", ETH_ALEN);
 384
 385         dev->flags |= IFF_NOARP;
 386     dev->features |= NETIF_F_NO_CSUM;
 387 }
 388
 389 int smsnet_module_init(void)
 390 {
 391         int rc;
 392
 393         INIT_LIST_HEAD(&g_smsnet_clients);
 394         kmutex_init(&g_smsnet_clientslock);
 395
 396         memset(&g_smsnet_stats, 0, sizeof(g_smsnet_stats));
 397
 398         g_smsnet_device = alloc_netdev(0, "sms", smsnet_setup_device);
 399         if (!g_smsnet_device)
 400         {
 401                 printk(KERN_INFO "%s alloc_netdev() failed\n", __FUNCTION__);
 402                 return -ENOMEM;
 403         }
 404
 405         rc = register_netdev(g_smsnet_device);
 406         if (rc < 0)
 407         {
 408                 printk(KERN_INFO "%s register_netdev() failed %d\n", __FUNCTION__, rc);
 409                 free_netdev(g_smsnet_device);
 410                 return rc;
 411         }
 412
 413         rc = smscore_register_hotplug(smsnet_hotplug);
 414
 415         printk(KERN_INFO "%s, rc %d\n", __FUNCTION__, rc);
 416
 417         return rc;
 418 }
 419
 420 void smsnet_module_exit(void)
 421 {
 422         if (g_smsnet_device)
 423         {
 424                 unregister_netdev(g_smsnet_device);
 425                 free_netdev(g_smsnet_device);
 426
 427                 g_smsnet_device = NULL;
 428         }
 429
 430         smscore_unregister_hotplug(smsnet_hotplug);
 431
 432         kmutex_lock(&g_smsnet_clientslock);
 433
 434         while (!list_empty(&g_smsnet_clients))
 435                 smsnet_unregister_client((smsnet_client_t*) g_smsnet_clients.next);
 436
 437         kmutex_unlock(&g_smsnet_clientslock);
 438
 439         printk(KERN_INFO "%s\n", __FUNCTION__);
 440 }
 441
 442 module_init(smsnet_module_init);
 443 module_exit(smsnet_module_exit);
 444
 445 MODULE_DESCRIPTION("smsnet dvb-h ip sink module");
 446 MODULE_AUTHOR("Anatoly Greenblatt,,, (anatolyg@siano-ms.com)");
 447 MODULE_LICENSE("GPL");