ARM: tegra12: set CPU rate to 2.2GHz for sku 0x87
[linux-3.10.git] / arch / arm / mach-tegra / tegra_wakeup_monitor.c
1 /*
2  * arch/arm/mach-tegra/tegra_wakeup_monitor.c
3  *
4  * Copyright (c) 2012-2013, NVIDIA CORPORATION.  All rights reserved.
5  *
6  * This program is free software; you can redistribute it and/or modify it
7  * under the terms and conditions of the GNU General Public License,
8  * version 2, as published by the Free Software Foundation.
9  *
10  * This program is distributed in the hope it will be useful, but WITHOUT
11  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
12  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License for
13  * more details.
14  *
15  * You should have received a copy of the GNU General Public License
16  * along with this program.  If not, see <http://www.gnu.org/licenses/>.
17  */
18
19 #include <linux/module.h>
20 #include <linux/platform_device.h>
21 #include <linux/completion.h>
22 #include <linux/suspend.h>
23 #include <linux/slab.h>
24 #include <linux/irqchip/tegra.h>
25
26 #include <net/ip.h>
27 #include <linux/netfilter_ipv4.h>
28 #include <net/ipv6.h>
29 #include <linux/proc_fs.h>
30
31 #include <mach/tegra_wakeup_monitor.h>
32
33
34 #define MAX_PACKET_NUM 6
35 #define MONITOR_HTABLE_SIZE 256
36 #define TWM_TCP_IPV4 1
37 #define TWM_UDP_IPV4 2
38 #define TWM_TCP_IPV6 3
39 #define TWM_UDP_IPV6 4
40 #define CMDLINE_LENGTH 80
41
42 struct packet_info {
43         struct hlist_nulls_node node;
44         unsigned short          dport;
45         atomic_t                *wakeup;
46         atomic_t                unmonitored;
47         unsigned int            valid_counter;
48 };
49
50 struct program_info {
51         struct hlist_nulls_node node;
52         unsigned int            hash;
53         char program[CMDLINE_LENGTH];
54         atomic_t                wakeup;
55         unsigned int            valid_counter;
56 };
57
58 struct twm_hslot {
59         struct hlist_nulls_head head;
60         int                     count;
61         spinlock_t              lock;
62 };
63
64 struct tegra_wakeup_monitor {
65                 struct tegra_wakeup_monitor_platform_data *pdata;
66                 struct notifier_block pm_notifier;
67                 struct platform_device *pdev;
68                 bool monitor_enable;
69                 bool nf_enable;
70                 bool am_enable;
71                 int wakeup_source;
72                 struct completion suspend_prepare_done;
73 };
74
75 struct _monitor_table {
76         struct twm_hslot                *program_hash;
77         struct twm_hslot                *tcp_hash;
78         struct twm_hslot                *udp_hash;
79         unsigned short                  mask;
80         unsigned int                    valid_counter;
81         struct tegra_wakeup_monitor     *twm;
82 };
83
84 static struct _monitor_table monitor_table;
85
86 static bool nf_monitor;
87 static bool nf_valid_flag;
88 static int nf_counter;
89
90 static unsigned int string_hash(const char *str)
91 {
92         unsigned int i, hash = 0;
93         for (i = 0; str[i] && i < CMDLINE_LENGTH; i++)
94                 hash = 31*hash + str[i];
95
96         return hash;
97 }
98
99 static ssize_t show_monitor_enable(struct device *dev,
100                                 struct device_attribute *attr, char *buf)
101 {
102         struct tegra_wakeup_monitor *twm = dev_get_drvdata(dev);
103         return sprintf(buf, "%d\n", twm->monitor_enable ? 1 : 0);
104 }
105
106 static ssize_t store_monitor_enable(struct device *dev,
107                                 struct device_attribute *attr,
108                                 const char *buf, size_t count)
109 {
110         int enable;
111         struct tegra_wakeup_monitor *twm = dev_get_drvdata(dev);
112
113         if (sscanf(buf, "%d", &enable) != 1 || enable < 0 || enable > 1)
114                 return -EINVAL;
115
116         dev_info(dev, "monitor enable = %d\n", enable);
117         twm->monitor_enable = enable;
118
119         return count;
120 }
121
122 /* MUST hold hslot->lock when call this func */
123 static struct program_info *get_program_info(struct device *dev,
124                         char *program,
125                         struct twm_hslot *hslot,
126                         bool auto_alloc)
127 {
128         struct program_info *cmd_info;
129         struct hlist_nulls_node *node;
130         if (hslot->count == 0)
131                 goto alloc;
132
133         hlist_nulls_for_each_entry(cmd_info, node, &hslot->head, node)
134                 if (!strncmp(cmd_info->program, program, CMDLINE_LENGTH))
135                         return cmd_info;
136 alloc:
137         if (!auto_alloc)
138                 return NULL;
139
140         cmd_info = devm_kzalloc(dev, sizeof(*cmd_info), GFP_ATOMIC);
141         if (!cmd_info) {
142                 dev_err(dev, "could not allocate a program_info");
143                 return NULL;
144         }
145
146         strncpy(cmd_info->program, program, CMDLINE_LENGTH);
147         cmd_info->hash = string_hash(program);
148         atomic_set(&cmd_info->wakeup, 0);
149         cmd_info->valid_counter = monitor_table.valid_counter;
150
151         hlist_nulls_add_head_rcu(&cmd_info->node, &hslot->head);
152         hslot->count++;
153         return cmd_info;
154 }
155
156 /* if the wakeup monitor is enabled, it will receive a command before suspend */
157 static ssize_t store_cmd(struct device *dev,
158                         struct device_attribute *attr,
159                         const char *buf, size_t count)
160 {
161         struct tegra_wakeup_monitor *twm = dev_get_drvdata(dev);
162
163         if (strncmp(buf, "0", 1))
164                 return -EINVAL;
165
166         dev_info(dev, "get done cmd\n");
167         complete(&twm->suspend_prepare_done);
168
169         return count;
170 }
171
172 static int tegra_wakeup_monitor_pm_notifier(struct notifier_block *notifier,
173                                    unsigned long pm_event, void *unused)
174 {
175         struct tegra_wakeup_monitor *twm =
176             container_of(notifier, struct tegra_wakeup_monitor, pm_notifier);
177         char *envp[2];
178         unsigned long const timeout =
179                         msecs_to_jiffies(TEGRA_WAKEUP_MONITOR_CMD_TIMEOUT_MS);
180
181         envp[1] = NULL;
182         switch (pm_event) {
183         case PM_SUSPEND_PREPARE:
184                 if (twm->monitor_enable) {
185                         dev_info(&twm->pdev->dev, "enter suspend_prepare\n");
186                         switch (twm->wakeup_source) {
187                         case TEGRA_WAKEUP_SOURCE_WIFI:
188                                 envp[0] = TEGRA_SUSPEND_PREPARE_UEVENT_WIFI;
189                                 break;
190                         default:
191                                 envp[0] = TEGRA_SUSPEND_PREPARE_UEVENT_OTHERS;
192                         }
193                         /* If we have more comletion, clean it */
194                         if (try_wait_for_completion(&twm->suspend_prepare_done))
195                                 dev_warn(&twm->pdev->dev,
196                                         "completion is not empty\n");
197                         /* send out a uevent to boardcast suspend prepare */
198                         kobject_uevent_env(&twm->pdev->dev.kobj, KOBJ_CHANGE,
199                                                 envp);
200                         /* clean the wakeup source flag */
201                         twm->wakeup_source = TEGRA_WAKEUP_SOURCE_OTHERS;
202
203                         /* waiting for cmd feedback */
204                         if (wait_for_completion_timeout(
205                                 &twm->suspend_prepare_done, timeout) == 0)
206                                 dev_err(&twm->pdev->dev, "cmd time out\n");
207                 }
208                 return NOTIFY_OK;
209         case PM_POST_SUSPEND:
210                 if (twm->monitor_enable) {
211                         dev_info(&twm->pdev->dev, "enter post_suspend\n");
212                         envp[0] = TEGRA_POST_SUSPEND_UEVENT;
213                         /* send out a uevent to boardcast post suspend*/
214                         kobject_uevent_env(&twm->pdev->dev.kobj, KOBJ_CHANGE,
215                                                 envp);
216                 }
217                 return NOTIFY_OK;
218         }
219
220         return NOTIFY_DONE;
221 }
222
223 /* MUST hold hslot->lock when call this func */
224 static struct packet_info *get_packet_info(struct device *dev,
225                         unsigned short port,
226                         struct twm_hslot *hslot,
227                         bool auto_alloc)
228 {
229         struct packet_info *packet_info;
230         struct hlist_nulls_node *node;
231         if (hslot->count == 0)
232                 goto alloc;
233
234         hlist_nulls_for_each_entry(packet_info, node, &hslot->head, node)
235                 if (packet_info->dport == port)
236                         return packet_info;
237
238 alloc:
239         if (!auto_alloc)
240                 return NULL;
241
242         packet_info = devm_kzalloc(dev, sizeof(*packet_info), GFP_ATOMIC);
243         if (!packet_info) {
244                 dev_err(dev, "could not allocate a packet_info");
245                 return NULL;
246         }
247
248         packet_info->dport = port;
249
250         hlist_nulls_add_head_rcu(&packet_info->node, &hslot->head);
251         hslot->count++;
252         return packet_info;
253 }
254
255 /* Note: this function must be SMP safe */
256 static unsigned int twm_nf_hook(unsigned int hook,
257                         struct sk_buff *skb,
258                         const struct net_device *indev,
259                         const struct net_device *outdev,
260                         int (*okfn) (struct sk_buff *))
261 {
262         struct tcphdr *ptcphdr, _tcph;
263         struct udphdr *pudphdr, _udph;
264         struct timeval tv;
265         unsigned short sport;
266         unsigned short dport;
267         unsigned char protocol;
268         __le16 fragoff;
269         int offset;
270         struct ipv6hdr _ipv6h, *ip6;
271         struct packet_info *packet_info;
272         struct twm_hslot *hslot_table, *hslot;
273         struct tegra_wakeup_monitor *twm = monitor_table.twm;
274
275         if (nf_monitor == false)
276                 return NF_ACCEPT;
277
278         if (!(skb) || !(ip_hdr(skb))) {
279                 dev_err(&twm->pdev->dev, "unexpected skb");
280                 return NF_ACCEPT;
281         }
282
283         if (nf_counter == 0)
284                 dev_dbg(&twm->pdev->dev, "a new begin of monitoring");
285
286         if (nf_counter++ >= MAX_PACKET_NUM)
287                 nf_monitor = false;
288
289         if (ip_hdr(skb)->version == 4) {
290                 protocol = ip_hdr(skb)->protocol;
291                 if (protocol == IPPROTO_TCP) {
292                         ptcphdr = (struct tcphdr *)
293                                 ((char *)ip_hdr(skb) + (ip_hdr(skb)->ihl << 2));
294                         sport = ntohs(ptcphdr->source);
295                         dport = ntohs(ptcphdr->dest);
296                         hslot_table = monitor_table.tcp_hash;
297                 } else if (protocol == IPPROTO_UDP) {
298                         pudphdr = (struct udphdr *)
299                                 ((char *)ip_hdr(skb) + (ip_hdr(skb)->ihl << 2));
300                         sport = ntohs(pudphdr->source);
301                         dport = ntohs(pudphdr->dest);
302                         hslot_table = monitor_table.udp_hash;
303                 } else {
304                         dev_dbg(&twm->pdev->dev, "unexpected transport layer trpacket");
305                         return NF_ACCEPT;
306                 }
307         } else if (ip_hdr(skb)->version == 6) {
308                 offset = skb_network_offset(skb);
309                 ip6 = skb_header_pointer(skb, offset,
310                         sizeof(struct ipv6hdr), &_ipv6h);
311                 if (ip6 == NULL)
312                         return NF_ACCEPT;
313
314                 protocol = ip6->nexthdr;
315                 offset += sizeof(struct ipv6hdr);
316                 offset = ipv6_skip_exthdr(skb, offset, &protocol, &fragoff);
317                 if (offset < 0)
318                         return NF_ACCEPT;
319
320                 switch (protocol) {
321                 case IPPROTO_TCP: {
322                         ptcphdr = skb_header_pointer(skb, offset,
323                                 sizeof(struct tcphdr), &_tcph);
324                         if (ptcphdr == NULL)
325                                 return NF_ACCEPT;
326
327                         sport = ntohs(ptcphdr->source);
328                         dport = ntohs(ptcphdr->dest);
329                         hslot_table = monitor_table.tcp_hash;
330                         break;
331                 }
332                 case IPPROTO_UDP: {
333                         pudphdr = skb_header_pointer(skb, offset,
334                                 sizeof(struct udphdr), &_udph);
335                         if (pudphdr == NULL)
336                                 return NF_ACCEPT;
337
338                         sport = ntohs(pudphdr->source);
339                         dport = ntohs(pudphdr->dest);
340                         hslot_table = monitor_table.udp_hash;
341                         break;
342                 }
343                 default:
344                         dev_dbg(&twm->pdev->dev, "unexpected transport layer packet");
345                         return NF_ACCEPT;
346                 }
347
348         } else {
349                 dev_dbg(&twm->pdev->dev, "unexpected not-IPV4/IPV6 packet");
350                 return NF_ACCEPT;
351         }
352
353         do_gettimeofday(&tv);
354
355         if (nf_valid_flag == true && nf_counter == 1) {
356                 hslot = &hslot_table[dport & monitor_table.mask];
357                 spin_lock_bh(&hslot->lock);
358                 packet_info = get_packet_info(&twm->pdev->dev,
359                                                 dport, hslot, true);
360                 if (packet_info == NULL) {
361                         dev_err(&twm->pdev->dev,
362                                 "%s: cannot get a packet_info", __func__);
363                         spin_unlock_bh(&hslot->lock);
364                         return NF_ACCEPT;
365                 } else if (packet_info->wakeup) {
366                         atomic_inc(packet_info->wakeup);
367                 } else {
368                         atomic_inc(&packet_info->unmonitored);
369                         packet_info->valid_counter =
370                                         monitor_table.valid_counter;
371                 }
372
373                 spin_unlock_bh(&hslot->lock);
374         }
375
376         dev_dbg(&twm->pdev->dev, "%d packet received from %s",
377                 nf_counter, indev->name);
378         dev_dbg(&twm->pdev->dev,
379                 "%s ip:%pI4 port:%u -> ip:%pI4 port:%u at time:%u,%u",
380                 (protocol == IPPROTO_TCP) ? "TCP" : "UDP",
381                 &ip_hdr(skb)->saddr, sport, &ip_hdr(skb)->daddr, dport,
382                 (unsigned int)tv.tv_sec, (unsigned int)tv.tv_usec);
383
384         return NF_ACCEPT;
385 }
386
387 static struct nf_hook_ops twm_nf_ops __read_mostly = {
388         .hook           = twm_nf_hook,
389         .pf             = PF_INET,
390         .hooknum        = NF_INET_PRE_ROUTING,
391         .priority       = NF_IP_PRI_MANGLE,
392 };
393
394 static ssize_t show_nf_enable(struct device *dev,
395                                 struct device_attribute *attr, char *buf)
396 {
397         struct tegra_wakeup_monitor *twm = dev_get_drvdata(dev);
398         return sprintf(buf, "%d\n", twm->nf_enable ? 1 : 0);
399 }
400
401 static ssize_t store_nf_enable(struct device *dev,
402                                 struct device_attribute *attr,
403                                 const char *buf, size_t count)
404 {
405         int err;
406         int enable;
407         struct tegra_wakeup_monitor *twm = dev_get_drvdata(dev);
408
409         if (sscanf(buf, "%d", &enable) != 1 || enable < 0 || enable > 1)
410                 return -EINVAL;
411
412         if (enable == 1 && twm->nf_enable == false) {
413                 err = nf_register_hook(&twm_nf_ops);
414                 if (err < 0) {
415                         dev_err(dev, "hook registration error\n");
416                         return err;
417                 }
418         } else if (enable == 0 && twm->nf_enable == true) {
419                 nf_unregister_hook(&twm_nf_ops);
420         }
421
422         twm->nf_enable = enable;
423         dev_info(dev, "netfilter enable = %d\n", enable);
424
425         return count;
426 }
427
428 static ssize_t show_am_enable(struct device *dev,
429                                 struct device_attribute *attr, char *buf)
430 {
431         struct tegra_wakeup_monitor *twm = dev_get_drvdata(dev);
432         return sprintf(buf, "%d\n", twm->am_enable ? 1 : 0);
433 }
434
435 static ssize_t store_am_enable(struct device *dev,
436                                 struct device_attribute *attr,
437                                 const char *buf, size_t count)
438 {
439         int enable;
440         struct tegra_wakeup_monitor *twm = dev_get_drvdata(dev);
441
442         if (sscanf(buf, "%d", &enable) != 1 || enable < 0 || enable > 1)
443                 return -EINVAL;
444
445         dev_info(dev, "am_enable = %d\n", enable);
446         twm->am_enable = enable;
447
448         return count;
449 }
450
451 static ssize_t store_init_ports(struct device *dev,
452                         struct device_attribute *attr,
453                         const char *buf, size_t count)
454 {
455         char *program = NULL;
456         unsigned int proto, port, off, hash;
457         struct program_info *cmd_info;
458         struct packet_info *packet_info;
459         struct twm_hslot *program_hslot_table, *pkt_hslot_table;
460         struct twm_hslot *program_hslot, *pkt_hslot;
461         struct tegra_wakeup_monitor *twm = dev_get_drvdata(dev);
462
463         if (!twm || !monitor_table.program_hash) {
464                 dev_err(dev, "monitor_table is not initialized!");
465                 return 0;
466         }
467
468         off = strlen(buf);
469         if (off >= PAGE_SIZE) {
470                 dev_err(dev, "buffer size is over %lu!", PAGE_SIZE);
471                 return 0;
472         }
473
474         program = kmalloc(PAGE_SIZE, GFP_KERNEL);
475         if (program == NULL) {
476                 dev_err(dev, "can't not alloate buffer!");
477                 return 0;
478         }
479
480         monitor_table.valid_counter++;
481         program_hslot_table = monitor_table.program_hash;
482         while (3 == sscanf(buf, "%s %u %u;%n", program, &proto, &port, &off)) {
483                 if (off == 0)
484                         break;
485                 buf += off;
486
487                 switch (proto) {
488                 case TWM_TCP_IPV4:
489                 case TWM_TCP_IPV6:
490                         pkt_hslot_table = monitor_table.tcp_hash;
491                         break;
492                 case TWM_UDP_IPV4:
493                 case TWM_UDP_IPV6:
494                         pkt_hslot_table = monitor_table.udp_hash;
495                         break;
496                 default:
497                         dev_err(dev, "%s: invalid proto type", __func__);
498                         continue;
499                 }
500
501                 if (port > 0xffff) {
502                         dev_err(dev, "%s: invalid port number", __func__);
503                         continue;
504                 }
505
506                 hash = string_hash(program);
507                 program_hslot = &program_hslot_table[hash & monitor_table.mask];
508                 spin_lock_bh(&program_hslot->lock);
509                 cmd_info = get_program_info(dev, program, program_hslot, true);
510                 if (cmd_info == NULL) {
511                         dev_err(dev, "%s: cannot get a program_info", __func__);
512                         spin_unlock_bh(&program_hslot->lock);
513                         continue;
514                 }
515                 cmd_info->hash = hash;
516                 atomic_set(&cmd_info->wakeup, 0);
517                 cmd_info->valid_counter = monitor_table.valid_counter;
518
519                 pkt_hslot = &pkt_hslot_table[port & monitor_table.mask];
520                 spin_lock_bh(&pkt_hslot->lock);
521                 packet_info = get_packet_info(dev, port, pkt_hslot, true);
522                 if (packet_info == NULL) {
523                         dev_err(dev, "%s: cannot get a packet_info", __func__);
524                         spin_unlock_bh(&pkt_hslot->lock);
525                         spin_unlock_bh(&program_hslot->lock);
526                         continue;
527                 }
528                 packet_info->dport = port;
529                 packet_info->wakeup = &cmd_info->wakeup;
530                 atomic_set(&packet_info->unmonitored, 0);
531                 packet_info->valid_counter = monitor_table.valid_counter;
532
533                 spin_unlock_bh(&pkt_hslot->lock);
534                 spin_unlock_bh(&program_hslot->lock);
535         }
536
537         kfree(program);
538         return count;
539 }
540
541 static ssize_t store_add_ports(struct device *dev,
542                         struct device_attribute *attr,
543                         const char *buf, size_t count)
544 {
545         char *program = NULL;
546         unsigned int proto, port, off, hash;
547         struct program_info *cmd_info;
548         struct packet_info *packet_info;
549         struct twm_hslot *program_hslot_table, *pkt_hslot_table;
550         struct twm_hslot *program_hslot, *pkt_hslot;
551         struct tegra_wakeup_monitor *twm = dev_get_drvdata(dev);
552
553         if (!twm || !monitor_table.program_hash) {
554                 dev_err(dev, "monitor_table is not initialized!");
555                 return 0;
556         }
557
558         off = strlen(buf);
559         if (off >= PAGE_SIZE) {
560                 dev_err(dev, "buffer size is over %lu!", PAGE_SIZE);
561                 return 0;
562         }
563
564         program = kmalloc(PAGE_SIZE, GFP_KERNEL);
565         if (program == NULL) {
566                 dev_err(dev, "can't not alloate buffer!");
567                 return 0;
568         }
569
570         program_hslot_table = monitor_table.program_hash;
571         while (3 == sscanf(buf, "%s %u %u;%n", program, &proto, &port, &off)) {
572                 if (off == 0)
573                         break;
574                 buf += off;
575
576                 switch (proto) {
577                 case TWM_TCP_IPV4:
578                 case TWM_TCP_IPV6:
579                         pkt_hslot_table = monitor_table.tcp_hash;
580                         break;
581                 case TWM_UDP_IPV4:
582                 case TWM_UDP_IPV6:
583                         pkt_hslot_table = monitor_table.udp_hash;
584                         break;
585                 default:
586                         dev_err(dev, "%s: invalid proto type", __func__);
587                         continue;
588                 }
589
590                 if (port > 0xffff) {
591                         dev_err(dev, "%s: invalid port number", __func__);
592                         continue;
593                 }
594
595                 hash = string_hash(program);
596                 program_hslot = &program_hslot_table[hash & monitor_table.mask];
597                 spin_lock_bh(&program_hslot->lock);
598                 cmd_info = get_program_info(dev, program, program_hslot, true);
599                 if (cmd_info == NULL) {
600                         dev_err(dev, "%s: cannot get a program_info", __func__);
601                         spin_unlock_bh(&program_hslot->lock);
602                         continue;
603                 }
604
605                 pkt_hslot = &pkt_hslot_table[port & monitor_table.mask];
606                 spin_lock_bh(&pkt_hslot->lock);
607                 packet_info = get_packet_info(dev, port, pkt_hslot, true);
608                 if (packet_info == NULL) {
609                         dev_err(dev, "%s: cannot get a packet_info", __func__);
610                         spin_unlock_bh(&pkt_hslot->lock);
611                         spin_unlock_bh(&program_hslot->lock);
612                         continue;
613                 }
614                 packet_info->dport = port;
615                 packet_info->wakeup = &cmd_info->wakeup;
616                 atomic_set(&packet_info->unmonitored, 0);
617                 packet_info->valid_counter = cmd_info->valid_counter;
618
619                 spin_unlock_bh(&pkt_hslot->lock);
620                 spin_unlock_bh(&program_hslot->lock);
621         }
622
623         kfree(program);
624         return count;
625 }
626
627 static ssize_t store_del_ports(struct device *dev,
628                         struct device_attribute *attr,
629                         const char *buf, size_t count)
630 {
631         char *program;
632         unsigned int proto, port, off, hash;
633         struct program_info *cmd_info, *target_program_info;
634         struct packet_info *packet_info;
635         struct twm_hslot *program_hslot_table, *pkt_hslot_table;
636         struct twm_hslot *program_hslot, *pkt_hslot;
637         struct tegra_wakeup_monitor *twm = dev_get_drvdata(dev);
638
639         if (!twm || !monitor_table.program_hash) {
640                 dev_err(dev, "monitor_table is not initialized!");
641                 return 0;
642         }
643
644         off = strlen(buf);
645         if (off >= PAGE_SIZE) {
646                 dev_err(dev, "buffer size is over %lu!", PAGE_SIZE);
647                 return 0;
648         }
649
650         program = kmalloc(PAGE_SIZE, GFP_KERNEL);
651         if (program == NULL) {
652                 dev_err(dev, "can't not alloate buffer!");
653                 return 0;
654         }
655
656         program_hslot_table = monitor_table.program_hash;
657         while (3 == sscanf(buf, "%s %u %u;%n",
658                 program, &proto, &port, &off)) {
659                 if (off == 0)
660                         break;
661                 buf += off;
662
663                 switch (proto) {
664                 case TWM_TCP_IPV4:
665                 case TWM_TCP_IPV6:
666                         pkt_hslot_table = monitor_table.tcp_hash;
667                         break;
668                 case TWM_UDP_IPV4:
669                 case TWM_UDP_IPV6:
670                         pkt_hslot_table = monitor_table.udp_hash;
671                         break;
672                 default:
673                         dev_err(dev, "%s: invalid proto type", __func__);
674                         continue;
675                 }
676
677                 if (port > 0xffff) {
678                         dev_err(dev, "%s: invalid port number", __func__);
679                         continue;
680                 }
681
682                 hash = string_hash(program);
683                 program_hslot = &program_hslot_table[hash & monitor_table.mask];
684                 spin_lock_bh(&program_hslot->lock);
685                 cmd_info = get_program_info(dev, program, program_hslot, false);
686                 if (cmd_info == NULL) {
687                         dev_err(dev, "%s: cannot get a program_info", __func__);
688                         spin_unlock_bh(&program_hslot->lock);
689                         continue;
690                 }
691
692                 pkt_hslot = &pkt_hslot_table[port & monitor_table.mask];
693                 spin_lock_bh(&pkt_hslot->lock);
694                 packet_info = get_packet_info(dev, port, pkt_hslot, false);
695                 if (packet_info == NULL) {
696                         dev_err(dev, "%s: cannot get a packet_info", __func__);
697                         spin_unlock_bh(&pkt_hslot->lock);
698                         spin_unlock_bh(&program_hslot->lock);
699                         continue;
700                 }
701                 target_program_info = container_of(packet_info->wakeup,
702                                         struct program_info, wakeup);
703                 if (!strncmp(target_program_info->program,
704                         program, CMDLINE_LENGTH)) {
705
706                         hlist_nulls_del(&packet_info->node);
707                         devm_kfree(dev, packet_info);
708                         pkt_hslot->count--;
709                 }
710
711                 spin_unlock_bh(&pkt_hslot->lock);
712                 spin_unlock_bh(&program_hslot->lock);
713         }
714
715         kfree(program);
716         return count;
717 }
718
719 static struct device_attribute twm_attrs[] = {
720         __ATTR(monitor_enable, S_IRUSR | S_IWUSR,
721                 show_monitor_enable, store_monitor_enable),
722         __ATTR(cmd, S_IWUSR, NULL, store_cmd),
723         __ATTR(nf_enable, S_IRUSR | S_IWUSR, show_nf_enable, store_nf_enable),
724         __ATTR(am_enable, S_IRUSR | S_IWUSR, show_am_enable, store_am_enable),
725         __ATTR(init_ports, S_IWUSR, NULL, store_init_ports),
726         __ATTR(add_ports, S_IWUSR, NULL, store_add_ports),
727         __ATTR(del_ports, S_IWUSR, NULL, store_del_ports),
728 };
729
730 static inline int monitor_table_init(struct _monitor_table *table,
731                                 struct tegra_wakeup_monitor *twm)
732 {
733         unsigned int i;
734
735         table->program_hash = devm_kzalloc(&twm->pdev->dev,
736                 MONITOR_HTABLE_SIZE *
737                 3 * sizeof(struct twm_hslot), GFP_KERNEL);
738
739         if (!table->program_hash)
740                 return -ENOMEM;
741
742         table->twm = twm;
743
744         table->mask = MONITOR_HTABLE_SIZE - 1;
745         table->tcp_hash = table->program_hash + (table->mask + 1);
746         table->udp_hash = table->tcp_hash + (table->mask + 1);
747         for (i = 0; i <= table->mask; i++) {
748                 INIT_HLIST_NULLS_HEAD(&table->program_hash[i].head, i);
749                 table->program_hash[i].count = 0;
750                 spin_lock_init(&table->program_hash[i].lock);
751         }
752         for (i = 0; i <= table->mask; i++) {
753                 INIT_HLIST_NULLS_HEAD(&table->tcp_hash[i].head, i);
754                 table->tcp_hash[i].count = 0;
755                 spin_lock_init(&table->tcp_hash[i].lock);
756         }
757         for (i = 0; i <= table->mask; i++) {
758                 INIT_HLIST_NULLS_HEAD(&table->udp_hash[i].head, i);
759                 table->udp_hash[i].count = 0;
760                 spin_lock_init(&table->udp_hash[i].lock);
761         }
762
763         return 0;
764 }
765
766 static int twm_offender_stat_show(struct seq_file *m, void *v)
767 {
768         int i = 0;
769         struct twm_hslot *hslot;
770         struct program_info *cmd_info;
771         struct packet_info *packet_info;
772         struct hlist_nulls_node *node;
773         struct device *dev;
774
775         if (!monitor_table.twm || !monitor_table.program_hash) {
776                 seq_printf(m, "monitor table not initialized\n");
777                 return 0;
778         }
779
780         dev = &monitor_table.twm->pdev->dev;
781
782         /* make sure the len does not exceed PAGE_SIZE */
783         for (i = 0; i <= monitor_table.mask; i++) {
784                 hslot = &monitor_table.program_hash[i];
785                 if (hslot->count == 0)
786                         continue;
787
788                 spin_lock_bh(&hslot->lock);
789                 hlist_nulls_for_each_entry(cmd_info, node,
790                                         &hslot->head, node) {
791                         if (cmd_info->valid_counter !=
792                                         monitor_table.valid_counter) {
793                                 hlist_nulls_del(&cmd_info->node);
794                                 devm_kfree(dev, cmd_info);
795                                 hslot->count--;
796                                 continue;
797                         }
798                         seq_printf(m, "program %s, wakeup times %u\n",
799                                 cmd_info->program,
800                                 atomic_read(&cmd_info->wakeup));
801                 }
802                 spin_unlock_bh(&hslot->lock);
803         }
804         seq_printf(m, "Unmonitored wakeups\nTCP statistics:\n");
805         for (i = 0; i <= monitor_table.mask; i++) {
806                 hslot = &monitor_table.tcp_hash[i];
807                 if (hslot->count == 0)
808                         continue;
809
810                 spin_lock_bh(&hslot->lock);
811                 hlist_nulls_for_each_entry(packet_info, node,
812                                         &hslot->head, node) {
813                         if (packet_info->valid_counter !=
814                                         monitor_table.valid_counter) {
815                                 hlist_nulls_del(&packet_info->node);
816                                 devm_kfree(dev, packet_info);
817                                 hslot->count--;
818                                 continue;
819                         }
820                         if (packet_info->wakeup)
821                                 continue;
822                         seq_printf(m, "port %u, wakeup times %u\n",
823                                 packet_info->dport,
824                                 atomic_read(&packet_info->unmonitored));
825                 }
826                 spin_unlock_bh(&hslot->lock);
827         }
828         seq_printf(m, "UDP statistics:\n");
829         for (i = 0; i <= monitor_table.mask; i++) {
830                 hslot = &monitor_table.udp_hash[i];
831                 if (hslot->count == 0)
832                         continue;
833
834                 spin_lock_bh(&hslot->lock);
835                 hlist_nulls_for_each_entry(packet_info, node,
836                                         &hslot->head, node) {
837                         if (packet_info->valid_counter !=
838                                         monitor_table.valid_counter) {
839                                 hlist_nulls_del(&packet_info->node);
840                                 devm_kfree(dev, packet_info);
841                                 hslot->count--;
842                                 continue;
843                         }
844                         if (packet_info->wakeup)
845                                 continue;
846                         seq_printf(m, "port %u, wakeup times %u\n",
847                                 packet_info->dport,
848                                 atomic_read(&packet_info->unmonitored));
849                 }
850                 spin_unlock_bh(&hslot->lock);
851         }
852
853         return 0;
854 }
855
856 static int twm_offender_stat_open(struct inode *inode, struct file *file) {
857         return single_open(file, twm_offender_stat_show, PDE_DATA(inode));
858 }
859
860 static const struct file_operations twm_offender_stat_fops = {
861         .open           = twm_offender_stat_open,
862         .read           = seq_read,
863         .llseek         = seq_lseek,
864         .release        = seq_release,
865 };
866
867 static inline int twm_init(struct tegra_wakeup_monitor *twm,
868                                         struct platform_device *pdev)
869 {
870         unsigned int i;
871         struct tegra_wakeup_monitor_platform_data *pdata =
872                 pdev->dev.platform_data;
873         int ret = 0;
874
875         twm->pdata = pdata;
876         twm->pdev  = pdev;
877         twm->monitor_enable = false;
878         twm->nf_enable = false;
879         twm->wakeup_source = TEGRA_WAKEUP_SOURCE_OTHERS;
880         twm->pm_notifier.notifier_call = tegra_wakeup_monitor_pm_notifier;
881         init_completion(&(twm->suspend_prepare_done));
882
883         ret = register_pm_notifier(&twm->pm_notifier);
884         if (ret < 0) {
885                 dev_err(&pdev->dev, "pm notifier registration error\n");
886                 return ret;
887         }
888
889         if (!proc_create("twm_offender_stat", 0, init_net.proc_net,
890                         &twm_offender_stat_fops)) {
891                 dev_err(&pdev->dev, "proc entry creation error\n");
892                 return -ENOMEM;
893         }
894
895         /* create sysfs node */
896         for (i = 0; i < ARRAY_SIZE(twm_attrs); i++) {
897                 ret = device_create_file(&pdev->dev, &twm_attrs[i]);
898                 if (ret)
899                         goto error;
900         }
901
902         return ret;
903
904 error:
905         remove_proc_entry("twm_offender_stat", init_net.proc_net);
906         for (i = 0; i < ARRAY_SIZE(twm_attrs); i++)
907                 device_remove_file(&pdev->dev, &twm_attrs[i]);
908
909         return ret;
910 }
911
912 static int tegra_wakeup_monitor_probe(struct platform_device *pdev)
913 {
914         struct tegra_wakeup_monitor_platform_data *pdata =
915             pdev->dev.platform_data;
916         struct tegra_wakeup_monitor *twm;
917         int ret = 0;
918
919         if (!pdata) {
920                 dev_dbg(&pdev->dev, "platform_data not available\n");
921                 return -EINVAL;
922         }
923
924         twm = devm_kzalloc(&pdev->dev,
925                 sizeof(struct tegra_wakeup_monitor), GFP_KERNEL);
926         if (!twm) {
927                 dev_dbg(&pdev->dev, "failed to allocate memory\n");
928                 return -ENOMEM;
929         }
930
931         ret = twm_init(twm, pdev);
932         if (ret < 0) {
933                 dev_err(&pdev->dev, "failed to init twm\n");
934                 goto error;
935         }
936
937         ret = dev_set_drvdata(&pdev->dev, twm);
938         if (ret < 0) {
939                 dev_err(&pdev->dev, "failed to set driver data\n");
940                 goto error;
941         }
942
943         ret = monitor_table_init(&monitor_table, twm);
944         if (ret < 0) {
945                 dev_err(&pdev->dev, "failed to init monitor table\n");
946                 goto error;
947         }
948
949         return 0;
950
951 error:
952         devm_kfree(&pdev->dev, twm);
953         return ret;
954 }
955
956 static int __exit tegra_wakeup_monitor_remove(struct platform_device *pdev)
957 {
958         unsigned int i;
959         struct tegra_wakeup_monitor *twm = platform_get_drvdata(pdev);
960
961         unregister_pm_notifier(&twm->pm_notifier);
962         nf_unregister_hook(&twm_nf_ops);
963
964         for (i = 0; i < ARRAY_SIZE(twm_attrs); i++)
965                 device_remove_file(&pdev->dev, &twm_attrs[i]);
966         remove_proc_entry("twm_offender_stat", init_net.proc_net);
967
968         kfree(twm); /* is it needed? */
969         return 0;
970 }
971
972 static int tegra_wakeup_monitor_suspend(struct platform_device *pdev,
973         pm_message_t state)
974 {
975         struct tegra_wakeup_monitor *twm = platform_get_drvdata(pdev);
976         if (twm->nf_enable) {
977                 nf_monitor = true;
978                 dev_info(&pdev->dev, "start netfilter monitoring\n");
979         } else {
980                 nf_monitor = false;
981         }
982
983         nf_counter = 0;
984
985         return 0;
986 }
987
988 static int tegra_wakeup_monitor_resume(struct platform_device *pdev)
989 {
990         struct tegra_wakeup_monitor *twm = platform_get_drvdata(pdev);
991
992         /* read and save wake status */
993         u64 wake_status = tegra_read_pmc_wake_status();
994         nf_valid_flag = false;
995         if (twm->pdata->wifi_wakeup_source != -1) {
996                 if (wake_status & BIT(twm->pdata->wifi_wakeup_source)) {
997                         twm->wakeup_source = TEGRA_WAKEUP_SOURCE_WIFI;
998                         nf_valid_flag = true;
999                 } else if (wake_status & BIT(twm->pdata->rtc_wakeup_source)) {
1000                         twm->wakeup_source = TEGRA_WAKEUP_SOURCE_RTC;
1001                         if (twm->am_enable)
1002                                 set_rtc_wakeup_src(1);
1003                 } else {
1004                         twm->wakeup_source = TEGRA_WAKEUP_SOURCE_OTHERS;
1005                 }
1006         } else {
1007                 twm->wakeup_source = TEGRA_WAKEUP_SOURCE_OTHERS;
1008         }
1009
1010         dev_info(&pdev->dev, "wakeup source = %d\n", twm->wakeup_source);
1011         return 0;
1012 }
1013
1014
1015 static struct platform_driver tegra_wakeup_monitor_driver = {
1016         .driver = {
1017                    .name = "tegra_wakeup_monitor",
1018                    .owner = THIS_MODULE,
1019                    },
1020         .probe = tegra_wakeup_monitor_probe,
1021         .remove = __exit_p(tegra_wakeup_monitor_remove),
1022 #ifdef CONFIG_PM
1023         .suspend = tegra_wakeup_monitor_suspend,
1024         .resume = tegra_wakeup_monitor_resume,
1025 #endif
1026 };
1027
1028 static int __init tegra_wakeup_monitor_init(void)
1029 {
1030         return platform_driver_register(&tegra_wakeup_monitor_driver);
1031 }
1032
1033 subsys_initcall(tegra_wakeup_monitor_init);
1034
1035 static void __exit tegra_wakeup_monitor_exit(void)
1036 {
1037         platform_driver_unregister(&tegra_wakeup_monitor_driver);
1038 }
1039
1040 module_exit(tegra_wakeup_monitor_exit);
1041
1042 MODULE_DESCRIPTION("Tegra Wakeup Monitor driver");
1043 MODULE_LICENSE("GPL");