energy/energy.c

   1 /*
   2  *  Dynamic Binary Instrumentation Module based on KProbes
   3  *  modules/energy/swap_energy.c
   4  *
   5  * This program is free software; you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License as published by
   7  * the Free Software Foundation; either version 2 of the License, or
   8  * (at your option) any later version.
   9  *
  10  * This program is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  * GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this program; if not, write to the Free Software
  17  * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA.
  18  *
  19  * Copyright (C) Samsung Electronics, 2013
  20  *
  21  * 2013         Vasiliy Ulyanov <v.ulyanov@samsung.com>
  22  *              Vyacheslav Cherkashin <v.cherkashin@samsung.com>
  23  *
  24  */
  25
  26
  27 #include <linux/module.h>
  28 #include <linux/file.h>
  29 #include <linux/spinlock.h>
  30 #include <linux/magic.h>
  31 #include <linux/slab.h>
  32 #include <linux/spinlock.h>
  33 #include <linux/net.h>
  34 #include <linux/socket.h>
  35 #include <linux/skbuff.h>
  36 #include <linux/string.h>
  37 #include <linux/fdtable.h>
  38 #include <net/sock.h>
  39 #include <kprobe/swap_kprobes.h>
  40 #include <ksyms/ksyms.h>
  41 #include <master/swap_deps.h>
  42 #include <us_manager/sspt/sspt_proc.h>
  43 #include <us_manager/sspt/sspt_feature.h>
  44 #include <linux/atomic.h>
  45 #include "energy.h"
  46 #include "lcd/lcd_base.h"
  47 #include "tm_stat.h"
  48
  49
  50 /* ============================================================================
  51  * =                              ENERGY_XXX                                  =
  52  * ============================================================================
  53  */
  54 struct kern_probe {
  55         const char *name;
  56         struct kretprobe *rp;
  57 };
  58
  59 static int energy_xxx_once(struct kern_probe p[], int size)
  60 {
  61         int i;
  62         const char *sym;
  63
  64         for (i = 0; i < size; ++i) {
  65                 struct kretprobe *rp = p[i].rp;
  66
  67                 sym = p[i].name;
  68                 rp->kp.addr = (kprobe_opcode_t *)swap_ksyms(sym);
  69                 if (rp->kp.addr == NULL)
  70                         goto not_found;
  71         }
  72
  73         return 0;
  74
  75 not_found:
  76         printk(KERN_INFO "ERROR: symbol '%s' not found\n", sym);
  77         return -ESRCH;
  78 }
  79
  80 static int energy_xxx_set(struct kern_probe p[], int size, int *flag)
  81 {
  82         int i, ret;
  83
  84         for (i = 0; i < size; ++i) {
  85                 ret = swap_register_kretprobe(p[i].rp);
  86                 if (ret)
  87                         goto fail;
  88         }
  89
  90         *flag = 1;
  91         return 0;
  92
  93 fail:
  94         pr_err("swap_register_kretprobe(%s) ret=%d\n", p[i].name, ret);
  95
  96         for (--i; i != -1; --i)
  97                 swap_unregister_kretprobe(p[i].rp);
  98
  99         return ret;
 100 }
 101
 102 static void energy_xxx_unset(struct kern_probe p[], int size, int *flag)
 103 {
 104         int i;
 105
 106         if (*flag == 0)
 107                 return;
 108
 109         for (i = size - 1; i != -1; --i)
 110                 swap_unregister_kretprobe(p[i].rp);
 111
 112         *flag = 0;
 113 }
 114
 115
 116
 117
 118
 119 /* ============================================================================
 120  * =                              CPUS_TIME                                   =
 121  * ============================================================================
 122  */
 123 struct cpus_time {
 124         spinlock_t lock; /* for concurrent access */
 125         struct tm_stat tm[NR_CPUS];
 126 };
 127
 128 #define cpus_time_lock(ct, flags) spin_lock_irqsave(&(ct)->lock, flags)
 129 #define cpus_time_unlock(ct, flags) spin_unlock_irqrestore(&(ct)->lock, flags)
 130
 131 static void cpus_time_init(struct cpus_time *ct, u64 time)
 132 {
 133         int cpu;
 134
 135         spin_lock_init(&ct->lock);
 136
 137         for (cpu = 0; cpu < NR_CPUS; ++cpu) {
 138                 tm_stat_init(&ct->tm[cpu]);
 139                 tm_stat_set_timestamp(&ct->tm[cpu], time);
 140         }
 141 }
 142
 143 static inline u64 cpu_time_get_running(struct cpus_time *ct, int cpu, u64 now)
 144 {
 145         return tm_stat_current_running(&ct->tm[cpu], now);
 146 }
 147
 148 static void *cpus_time_get_running_all(struct cpus_time *ct, u64 *buf, u64 now)
 149 {
 150         int cpu;
 151
 152         for (cpu = 0; cpu < NR_CPUS; ++cpu)
 153                 buf[cpu] = tm_stat_current_running(&ct->tm[cpu], now);
 154
 155         return buf;
 156 }
 157
 158 static void *cpus_time_sum_running_all(struct cpus_time *ct, u64 *buf, u64 now)
 159 {
 160         int cpu;
 161
 162         for (cpu = 0; cpu < NR_CPUS; ++cpu)
 163                 buf[cpu] += tm_stat_current_running(&ct->tm[cpu], now);
 164
 165         return buf;
 166 }
 167
 168 static void cpus_time_save_entry(struct cpus_time *ct, int cpu, u64 time)
 169 {
 170         struct tm_stat *tm = &ct->tm[cpu];
 171
 172         if (unlikely(tm_stat_timestamp(tm))) /* should never happen */
 173                 printk("XXX %s[%d/%d]: WARNING tmstamp(%p) set on cpu(%d)\n",
 174                        current->comm, current->tgid, current->pid, tm, cpu);
 175         tm_stat_set_timestamp(&ct->tm[cpu], time);
 176 }
 177
 178 static void cpus_time_update_running(struct cpus_time *ct, int cpu, u64 now,
 179                                      u64 start_time)
 180 {
 181         struct tm_stat *tm = &ct->tm[cpu];
 182
 183         if (unlikely(tm_stat_timestamp(tm) == 0)) {
 184                  /* not initialized. should happen only once per cpu/task */
 185                 printk("XXX %s[%d/%d]: nnitializing tmstamp(%p) on cpu(%d)\n",
 186                        current->comm, current->tgid, current->pid, tm, cpu);
 187                 tm_stat_set_timestamp(tm, start_time);
 188         }
 189
 190         tm_stat_update(tm, now);
 191         tm_stat_set_timestamp(tm, 0); /* set timestamp to 0 */
 192 }
 193
 194
 195
 196
 197
 198 struct energy_data {
 199         /* for __switch_to */
 200         struct cpus_time ct;
 201
 202         /* for sys_read */
 203         atomic64_t bytes_read;
 204
 205         /*for sys_write */
 206         atomic64_t bytes_written;
 207
 208         /*for recvmsg*/
 209         atomic64_t bytes_recv;
 210
 211         /* for sock_send */
 212         atomic64_t bytes_send;
 213
 214         /* for l2cap_recv */
 215         atomic64_t bytes_l2cap_recv_acldata;
 216
 217         /* for sco_recv_scodata */
 218         atomic64_t bytes_sco_recv_scodata;
 219
 220         /* for hci_send_acl */
 221         atomic64_t bytes_hci_send_acl;
 222
 223         /* for hci_send_sco */
 224         atomic64_t bytes_hci_send_sco;
 225 };
 226
 227 static sspt_feature_id_t feature_id = SSPT_FEATURE_ID_BAD;
 228
 229 static void init_ed(struct energy_data *ed)
 230 {
 231         /* instead of get_ntime(), CPU time is initialized to 0 here. Timestamp
 232          * value will be properly set when the corresponding __switch_to event
 233          * occurs */
 234         cpus_time_init(&ed->ct, 0);
 235         atomic64_set(&ed->bytes_read, 0);
 236         atomic64_set(&ed->bytes_written, 0);
 237         atomic64_set(&ed->bytes_recv, 0);
 238         atomic64_set(&ed->bytes_send, 0);
 239         atomic64_set(&ed->bytes_l2cap_recv_acldata, 0);
 240         atomic64_set(&ed->bytes_sco_recv_scodata, 0);
 241         atomic64_set(&ed->bytes_hci_send_acl, 0);
 242         atomic64_set(&ed->bytes_hci_send_sco, 0);
 243 }
 244
 245 static void uninit_ed(struct energy_data *ed)
 246 {
 247         cpus_time_init(&ed->ct, 0);
 248         atomic64_set(&ed->bytes_read, 0);
 249         atomic64_set(&ed->bytes_written, 0);
 250         atomic64_set(&ed->bytes_recv, 0);
 251         atomic64_set(&ed->bytes_send, 0);
 252         atomic64_set(&ed->bytes_l2cap_recv_acldata, 0);
 253         atomic64_set(&ed->bytes_sco_recv_scodata, 0);
 254         atomic64_set(&ed->bytes_hci_send_acl, 0);
 255         atomic64_set(&ed->bytes_hci_send_sco, 0);
 256 }
 257
 258 static void *create_ed(void)
 259 {
 260         struct energy_data *ed;
 261
 262         ed = kmalloc(sizeof(*ed), GFP_ATOMIC);
 263         if (ed)
 264                 init_ed(ed);
 265
 266         return (void *)ed;
 267 }
 268
 269 static void destroy_ed(void *data)
 270 {
 271         struct energy_data *ed = (struct energy_data *)data;
 272         kfree(ed);
 273 }
 274
 275
 276 static int init_feature(void)
 277 {
 278         feature_id = sspt_register_feature(create_ed, destroy_ed);
 279
 280         if (feature_id == SSPT_FEATURE_ID_BAD)
 281                 return -EPERM;
 282
 283         return 0;
 284 }
 285
 286 static void uninit_feature(void)
 287 {
 288         sspt_unregister_feature(feature_id);
 289         feature_id = SSPT_FEATURE_ID_BAD;
 290 }
 291
 292 static struct energy_data *get_energy_data(struct task_struct *task)
 293 {
 294         void *data = NULL;
 295         struct sspt_proc *proc;
 296
 297         proc = sspt_proc_get_by_task(task);
 298         if (proc)
 299                 data = sspt_get_feature_data(proc->feature, feature_id);
 300
 301         return (struct energy_data *)data;
 302 }
 303
 304 static int check_fs(unsigned long magic)
 305 {
 306         switch (magic) {
 307         case EXT2_SUPER_MAGIC: /* == EXT3_SUPER_MAGIC == EXT4_SUPER_MAGIC */
 308         case MSDOS_SUPER_MAGIC:
 309                 return 1;
 310         }
 311
 312         return 0;
 313 }
 314
 315 static int check_ftype(int fd)
 316 {
 317         int err, ret = 0;
 318         struct kstat kstat;
 319
 320         err = vfs_fstat(fd, &kstat);
 321         if (err == 0 && S_ISREG(kstat.mode))
 322                 ret = 1;
 323
 324         return ret;
 325 }
 326
 327 static int check_file(int fd)
 328 {
 329         struct file *file;
 330
 331         file = fget(fd);
 332         if (file) {
 333                 int magic = 0;
 334                 if (file->f_dentry && file->f_dentry->d_sb)
 335                         magic = file->f_dentry->d_sb->s_magic;
 336
 337                 fput(file);
 338
 339                 if (check_fs(magic) && check_ftype(fd))
 340                         return 1;
 341         }
 342
 343         return 0;
 344 }
 345
 346 static unsigned long get_arg0(struct pt_regs *regs)
 347 {
 348 #if defined(CONFIG_ARM)
 349         return regs->ARM_r0;
 350 #elif defined(CONFIG_X86_32)
 351         return regs->bx;
 352 #else
 353         #error "this architecture is not supported"
 354 #endif /* CONFIG_arch */
 355 }
 356
 357
 358
 359
 360
 361 static struct cpus_time ct_idle;
 362 static struct energy_data ed_system;
 363 static u64 start_time;
 364
 365 static void init_data_energy(void)
 366 {
 367         start_time = get_ntime();
 368         init_ed(&ed_system);
 369         cpus_time_init(&ct_idle, 0);
 370 }
 371
 372 static void uninit_data_energy(void)
 373 {
 374         start_time = 0;
 375         uninit_ed(&ed_system);
 376         cpus_time_init(&ct_idle, 0);
 377 }
 378
 379
 380
 381
 382
 383 /* ============================================================================
 384  * =                             __switch_to                                  =
 385  * ============================================================================
 386  */
 387 static int entry_handler_switch(struct kretprobe_instance *ri, struct pt_regs *regs)
 388 {
 389         int cpu;
 390         struct cpus_time *ct;
 391         struct energy_data *ed;
 392         unsigned long flags;
 393
 394         cpu = smp_processor_id();
 395
 396         ct = current->tgid ? &ed_system.ct: &ct_idle;
 397         cpus_time_lock(ct, flags);
 398         cpus_time_update_running(ct, cpu, get_ntime(), start_time);
 399         cpus_time_unlock(ct, flags);
 400
 401         ed = get_energy_data(current);
 402         if (ed) {
 403                 ct = &ed->ct;
 404                 cpus_time_lock(ct, flags);
 405                 cpus_time_update_running(ct, cpu, get_ntime(), start_time);
 406                 cpus_time_unlock(ct, flags);
 407         }
 408
 409         return 0;
 410 }
 411
 412 static int ret_handler_switch(struct kretprobe_instance *ri, struct pt_regs *regs)
 413 {
 414         int cpu;
 415         struct cpus_time *ct;
 416         struct energy_data *ed;
 417         unsigned long flags;
 418
 419         cpu = smp_processor_id();
 420
 421         ct = current->tgid ? &ed_system.ct: &ct_idle;
 422         cpus_time_lock(ct, flags);
 423         cpus_time_save_entry(ct, cpu, get_ntime());
 424         cpus_time_unlock(ct, flags);
 425
 426         ed = get_energy_data(current);
 427         if (ed) {
 428                 ct = &ed->ct;
 429                 cpus_time_lock(ct, flags);
 430                 cpus_time_save_entry(ct, cpu, get_ntime());
 431                 cpus_time_unlock(ct, flags);
 432         }
 433
 434         return 0;
 435 }
 436
 437 static struct kretprobe switch_to_krp = {
 438         .entry_handler = entry_handler_switch,
 439         .handler = ret_handler_switch,
 440 };
 441
 442
 443
 444
 445
 446 /* ============================================================================
 447  * =                                sys_read                                  =
 448  * ============================================================================
 449  */
 450 struct sys_read_data {
 451         int fd;
 452 };
 453
 454 static int entry_handler_sys_read(struct kretprobe_instance *ri, struct pt_regs *regs)
 455 {
 456         struct sys_read_data *srd = (struct sys_read_data *)ri->data;
 457
 458         srd->fd = (int)get_arg0(regs);
 459
 460         return 0;
 461 }
 462
 463 static int ret_handler_sys_read(struct kretprobe_instance *ri,
 464                                 struct pt_regs *regs)
 465 {
 466         int ret = regs_return_value(regs);
 467
 468         if (ret > 0) {
 469                 struct sys_read_data *srd;
 470
 471                 srd = (struct sys_read_data *)ri->data;
 472                 if (check_file(srd->fd)) {
 473                         struct energy_data *ed;
 474
 475                         ed = get_energy_data(current);
 476                         if (ed)
 477                                 atomic64_add(ret, &ed->bytes_read);
 478
 479                         atomic64_add(ret, &ed_system.bytes_read);
 480                 }
 481         }
 482
 483         return 0;
 484 }
 485
 486 static struct kretprobe sys_read_krp = {
 487         .entry_handler = entry_handler_sys_read,
 488         .handler = ret_handler_sys_read,
 489         .data_size = sizeof(struct sys_read_data)
 490 };
 491
 492
 493
 494
 495
 496 /* ============================================================================
 497  * =                               sys_write                                  =
 498  * ============================================================================
 499  */
 500 static int entry_handler_sys_write(struct kretprobe_instance *ri, struct pt_regs *regs)
 501 {
 502         struct sys_read_data *srd = (struct sys_read_data *)ri->data;
 503
 504         srd->fd = (int)get_arg0(regs);
 505
 506         return 0;
 507 }
 508
 509 static int ret_handler_sys_write(struct kretprobe_instance *ri, struct pt_regs *regs)
 510 {
 511         int ret = regs_return_value(regs);
 512
 513         if (ret > 0) {
 514                 struct sys_read_data *srd;
 515
 516                 srd = (struct sys_read_data *)ri->data;
 517                 if (check_file(srd->fd)) {
 518                         struct energy_data *ed;
 519
 520                         ed = get_energy_data(current);
 521                         if (ed)
 522                                 atomic64_add(ret, &ed->bytes_written);
 523
 524                         atomic64_add(ret, &ed_system.bytes_written);
 525                 }
 526         }
 527
 528         return 0;
 529 }
 530
 531 static struct kretprobe sys_write_krp = {
 532         .entry_handler = entry_handler_sys_write,
 533         .handler = ret_handler_sys_write,
 534         .data_size = sizeof(struct sys_read_data)
 535 };
 536
 537
 538
 539
 540
 541 /* ============================================================================
 542  * =                                wifi                                      =
 543  * ============================================================================
 544  */
 545 static bool check_wlan0(struct socket *sock)
 546 {
 547         /* FIXME: hardcode interface */
 548         const char *name_intrf = "wlan0";
 549
 550         if (sock->sk->sk_dst_cache &&
 551             sock->sk->sk_dst_cache->dev &&
 552             !strcmp(sock->sk->sk_dst_cache->dev->name, name_intrf))
 553                 return true;
 554
 555         return false;
 556 }
 557
 558 static bool check_socket(struct task_struct *task, struct socket *socket)
 559 {
 560         bool ret = false;
 561         unsigned int fd;
 562         struct files_struct *files;
 563
 564         files = swap_get_files_struct(task);
 565         if (files == NULL)
 566                 return false;
 567
 568         rcu_read_lock();
 569         for (fd = 0; fd < files_fdtable(files)->max_fds; ++fd) {
 570                 if (fcheck_files(files, fd) == socket->file) {
 571                         ret = true;
 572                         goto unlock;
 573                 }
 574         }
 575
 576 unlock:
 577         rcu_read_unlock();
 578         swap_put_files_struct(files);
 579         return ret;
 580 }
 581
 582 static struct energy_data *get_energy_data_by_socket(struct task_struct *task,
 583                                                      struct socket *socket)
 584 {
 585         struct energy_data *ed;
 586
 587         ed = get_energy_data(task);
 588         if (ed)
 589                 ed = check_socket(task, socket) ? ed : NULL;
 590
 591         return ed;
 592 }
 593
 594 static int wf_sock_eh(struct kretprobe_instance *ri, struct pt_regs *regs)
 595 {
 596         struct socket *socket = (struct socket *)swap_get_karg(regs, 0);
 597
 598         *(struct socket **)ri->data = check_wlan0(socket) ? socket : NULL;
 599
 600         return 0;
 601 }
 602
 603 static int wf_sock_aio_eh(struct kretprobe_instance *ri, struct pt_regs *regs)
 604 {
 605         struct kiocb *iocb = (struct kiocb *)swap_get_karg(regs, 0);
 606         struct socket *socket = iocb->ki_filp->private_data;
 607
 608         *(struct socket **)ri->data = check_wlan0(socket) ? socket : NULL;
 609
 610         return 0;
 611 }
 612
 613 static int wf_sock_recv_rh(struct kretprobe_instance *ri, struct pt_regs *regs)
 614 {
 615         int ret = regs_return_value(regs);
 616
 617         if (ret > 0) {
 618                 struct socket *socket = *(struct socket **)ri->data;
 619
 620                 if (socket) {
 621                         struct energy_data *ed;
 622
 623                         ed = get_energy_data_by_socket(current, socket);
 624                         if (ed)
 625                                 atomic64_add(ret, &ed->bytes_recv);
 626                         atomic64_add(ret, &ed_system.bytes_recv);
 627                 }
 628         }
 629
 630         return 0;
 631 }
 632
 633 static int wf_sock_send_rh(struct kretprobe_instance *ri, struct pt_regs *regs)
 634 {
 635         int ret = regs_return_value(regs);
 636
 637         if (ret > 0) {
 638                 struct socket *socket = *(struct socket **)ri->data;
 639
 640                 if (socket) {
 641                         struct energy_data *ed;
 642
 643                         ed = get_energy_data_by_socket(current, socket);
 644                         if (ed)
 645                                 atomic64_add(ret, &ed->bytes_send);
 646                         atomic64_add(ret, &ed_system.bytes_send);
 647                 }
 648         }
 649
 650         return 0;
 651 }
 652
 653 static struct kretprobe sock_recv_krp = {
 654         .entry_handler = wf_sock_eh,
 655         .handler = wf_sock_recv_rh,
 656         .data_size = sizeof(struct socket *)
 657 };
 658
 659 static struct kretprobe sock_send_krp = {
 660         .entry_handler = wf_sock_eh,
 661         .handler = wf_sock_send_rh,
 662         .data_size = sizeof(struct socket *)
 663 };
 664
 665 static struct kretprobe sock_aio_read_krp = {
 666         .entry_handler = wf_sock_aio_eh,
 667         .handler = wf_sock_recv_rh,
 668         .data_size = sizeof(struct socket *)
 669 };
 670
 671 static struct kretprobe sock_aio_write_krp = {
 672         .entry_handler = wf_sock_aio_eh,
 673         .handler = wf_sock_send_rh,
 674         .data_size = sizeof(struct socket *)
 675 };
 676
 677 static struct kern_probe wifi_probes[] = {
 678         {
 679                 .name = "sock_recvmsg",
 680                 .rp = &sock_recv_krp,
 681         },
 682         {
 683                 .name = "sock_sendmsg",
 684                 .rp = &sock_send_krp,
 685         },
 686         {
 687                 .name = "sock_aio_read",
 688                 .rp = &sock_aio_read_krp,
 689         },
 690         {
 691                 .name = "sock_aio_write",
 692                 .rp = &sock_aio_write_krp,
 693         }
 694 };
 695
 696 enum { wifi_probes_cnt = ARRAY_SIZE(wifi_probes) };
 697 static int wifi_flag = 0;
 698
 699
 700
 701
 702
 703 /* ============================================================================
 704  * =                                bluetooth                                 =
 705  * ============================================================================
 706  */
 707
 708 struct swap_bt_data {
 709         struct socket *socket;
 710 };
 711
 712 static int bt_entry_handler(struct kretprobe_instance *ri,
 713                             struct pt_regs *regs)
 714 {
 715         struct swap_bt_data *data = (struct swap_bt_data *)ri->data;
 716         struct socket *sock = (struct socket *)swap_get_sarg(regs, 1);
 717
 718         data->socket = sock ? sock : NULL;
 719
 720         return 0;
 721 }
 722
 723 static int bt_recvmsg_handler(struct kretprobe_instance *ri,
 724                               struct pt_regs *regs)
 725 {
 726         int ret = regs_return_value(regs);
 727         struct swap_bt_data *data = (struct swap_bt_data *)ri->data;
 728
 729         if (ret > 0) {
 730                 struct socket *socket = data->socket;
 731
 732                 if (socket) {
 733                         struct energy_data *ed;
 734
 735                         ed = get_energy_data_by_socket(current, socket);
 736                         if (ed)
 737                                 atomic64_add(ret, &ed->bytes_l2cap_recv_acldata);
 738                 }
 739                 atomic64_add(ret, &ed_system.bytes_l2cap_recv_acldata);
 740         }
 741
 742         return 0;
 743 }
 744
 745 static int bt_sendmsg_handler(struct kretprobe_instance *ri,
 746                               struct pt_regs *regs)
 747 {
 748         int ret = regs_return_value(regs);
 749         struct swap_bt_data *data = (struct swap_bt_data *)ri->data;
 750
 751         if (ret > 0) {
 752                 struct socket *socket = data->socket;
 753
 754                 if (socket) {
 755                         struct energy_data *ed;
 756
 757                         ed = get_energy_data_by_socket(current, socket);
 758                         if (ed)
 759                                 atomic64_add(ret, &ed->bytes_hci_send_sco);
 760                 }
 761                 atomic64_add(ret, &ed_system.bytes_hci_send_sco);
 762         }
 763
 764         return 0;
 765 }
 766
 767 static struct kretprobe rfcomm_sock_recvmsg_krp = {
 768         .entry_handler = bt_entry_handler,
 769         .handler = bt_recvmsg_handler,
 770         .data_size = sizeof(struct swap_bt_data)
 771 };
 772
 773 static struct kretprobe l2cap_sock_recvmsg_krp = {
 774         .entry_handler = bt_entry_handler,
 775         .handler = bt_recvmsg_handler,
 776         .data_size = sizeof(struct swap_bt_data)
 777 };
 778
 779 static struct kretprobe hci_sock_recvmsg_krp = {
 780         .entry_handler = bt_entry_handler,
 781         .handler = bt_recvmsg_handler,
 782         .data_size = sizeof(struct swap_bt_data)
 783 };
 784
 785 static struct kretprobe sco_sock_recvmsg_krp = {
 786         .entry_handler = bt_entry_handler,
 787         .handler = bt_recvmsg_handler,
 788         .data_size = sizeof(struct swap_bt_data)
 789 };
 790 static struct kretprobe rfcomm_sock_sendmsg_krp = {
 791         .entry_handler = bt_entry_handler,
 792         .handler = bt_sendmsg_handler,
 793         .data_size = sizeof(struct swap_bt_data)
 794 };
 795
 796 static struct kretprobe l2cap_sock_sendmsg_krp = {
 797         .entry_handler = bt_entry_handler,
 798         .handler = bt_sendmsg_handler,
 799         .data_size = sizeof(struct swap_bt_data)
 800 };
 801
 802 static struct kretprobe hci_sock_sendmsg_krp = {
 803         .entry_handler = bt_entry_handler,
 804         .handler = bt_sendmsg_handler,
 805         .data_size = sizeof(struct swap_bt_data)
 806 };
 807
 808 static struct kretprobe sco_sock_sendmsg_krp = {
 809         .entry_handler = bt_entry_handler,
 810         .handler = bt_sendmsg_handler,
 811         .data_size = sizeof(struct swap_bt_data)
 812 };
 813
 814 static struct kern_probe bt_probes[] = {
 815         {
 816                 .name = "rfcomm_sock_recvmsg",
 817                 .rp = &rfcomm_sock_recvmsg_krp,
 818         },
 819         {
 820                 .name = "l2cap_sock_recvmsg",
 821                 .rp = &l2cap_sock_recvmsg_krp,
 822         },
 823         {
 824                 .name = "hci_sock_recvmsg",
 825                 .rp = &hci_sock_recvmsg_krp,
 826         },
 827         {
 828                 .name = "sco_sock_recvmsg",
 829                 .rp = &sco_sock_recvmsg_krp,
 830         },
 831         {
 832                 .name = "rfcomm_sock_sendmsg",
 833                 .rp = &rfcomm_sock_sendmsg_krp,
 834         },
 835         {
 836                 .name = "l2cap_sock_sendmsg",
 837                 .rp = &l2cap_sock_sendmsg_krp,
 838         },
 839         {
 840                 .name = "hci_sock_sendmsg",
 841                 .rp = &hci_sock_sendmsg_krp,
 842         },
 843         {
 844                 .name = "sco_sock_sendmsg",
 845                 .rp = &sco_sock_sendmsg_krp,
 846         }
 847 };
 848
 849 enum { bt_probes_cnt = ARRAY_SIZE(bt_probes) };
 850 static int energy_bt_flag = 0;
 851
 852 enum parameter_type {
 853         PT_CPU,
 854         PT_READ,
 855         PT_WRITE,
 856         PT_WF_RECV,
 857         PT_WF_SEND,
 858         PT_L2CAP_RECV,
 859         PT_SCO_RECV,
 860         PT_SEND_ACL,
 861         PT_SEND_SCO
 862 };
 863
 864 struct cmd_pt {
 865         enum parameter_type pt;
 866         void *buf;
 867         int sz;
 868 };
 869
 870 static void callback_for_proc(struct sspt_proc *proc, void *data)
 871 {
 872         void *f_data = sspt_get_feature_data(proc->feature, feature_id);
 873         struct energy_data *ed = (struct energy_data *)f_data;
 874
 875         if (ed) {
 876                 unsigned long flags;
 877                 struct cmd_pt *cmdp = (struct cmd_pt *)data;
 878                 u64 *val = cmdp->buf;
 879
 880                 switch (cmdp->pt) {
 881                 case PT_CPU:
 882                         cpus_time_lock(&ed->ct, flags);
 883                         cpus_time_sum_running_all(&ed->ct, val, get_ntime());
 884                         cpus_time_unlock(&ed->ct, flags);
 885                         break;
 886                 case PT_READ:
 887                         *val += atomic64_read(&ed->bytes_read);
 888                         break;
 889                 case PT_WRITE:
 890                         *val += atomic64_read(&ed->bytes_written);
 891                         break;
 892                 case PT_WF_RECV:
 893                         *val += atomic64_read(&ed->bytes_recv);
 894                         break;
 895                 case PT_WF_SEND:
 896                         *val += atomic64_read(&ed->bytes_send);
 897                         break;
 898                 case PT_L2CAP_RECV:
 899                         *val += atomic64_read(&ed->bytes_l2cap_recv_acldata);
 900                         break;
 901                 case PT_SCO_RECV:
 902                         *val += atomic64_read(&ed->bytes_sco_recv_scodata);
 903                         break;
 904                 case PT_SEND_ACL:
 905                         *val += atomic64_read(&ed->bytes_hci_send_acl);
 906                         break;
 907                 case PT_SEND_SCO:
 908                         *val += atomic64_read(&ed->bytes_hci_send_sco);
 909                         break;
 910                 default:
 911                         break;
 912                 }
 913         }
 914 }
 915
 916 static int current_parameter_apps(enum parameter_type pt, void *buf, int sz)
 917 {
 918         struct cmd_pt cmdp;
 919
 920         cmdp.pt = pt;
 921         cmdp.buf = buf;
 922         cmdp.sz = sz;
 923
 924         on_each_proc(callback_for_proc, (void *)&cmdp);
 925
 926         return 0;
 927 }
 928
 929 /**
 930  * @brief Get energy parameter
 931  *
 932  * @param pe Type of energy parameter
 933  * @param buf Buffer
 934  * @param sz Buffer size
 935  * @return Error code
 936  */
 937 int get_parameter_energy(enum parameter_energy pe, void *buf, size_t sz)
 938 {
 939         unsigned long flags;
 940         u64 *val = buf; /* currently all parameters are u64 vals */
 941         int ret = 0;
 942
 943         switch (pe) {
 944         case PE_TIME_IDLE:
 945                 cpus_time_lock(&ct_idle, flags);
 946                 /* for the moment we consider only CPU[0] idle time */
 947                 *val = cpu_time_get_running(&ct_idle, 0, get_ntime());
 948                 cpus_time_unlock(&ct_idle, flags);
 949                 break;
 950         case PE_TIME_SYSTEM:
 951                 cpus_time_lock(&ed_system.ct, flags);
 952                 cpus_time_get_running_all(&ed_system.ct, val, get_ntime());
 953                 cpus_time_unlock(&ed_system.ct, flags);
 954                 break;
 955         case PE_TIME_APPS:
 956                 current_parameter_apps(PT_CPU, buf, sz);
 957                 break;
 958         case PE_READ_SYSTEM:
 959                 *val = atomic64_read(&ed_system.bytes_read);
 960                 break;
 961         case PE_WRITE_SYSTEM:
 962                 *val = atomic64_read(&ed_system.bytes_written);
 963                 break;
 964         case PE_WF_RECV_SYSTEM:
 965                 *val = atomic64_read(&ed_system.bytes_recv);
 966                 break;
 967         case PE_WF_SEND_SYSTEM:
 968                 *val = atomic64_read(&ed_system.bytes_send);
 969                 break;
 970         case PE_L2CAP_RECV_SYSTEM:
 971                 *val = atomic64_read(&ed_system.bytes_l2cap_recv_acldata);
 972                 break;
 973         case PE_SCO_RECV_SYSTEM:
 974                 *val = atomic64_read(&ed_system.bytes_sco_recv_scodata);
 975                 break;
 976         case PT_SEND_ACL_SYSTEM:
 977                 *val = atomic64_read(&ed_system.bytes_hci_send_acl);
 978                 break;
 979         case PT_SEND_SCO_SYSTEM:
 980                 *val = atomic64_read(&ed_system.bytes_hci_send_sco);
 981                 break;
 982         case PE_READ_APPS:
 983                 current_parameter_apps(PT_READ, buf, sz);
 984                 break;
 985         case PE_WRITE_APPS:
 986                 current_parameter_apps(PT_WRITE, buf, sz);
 987                 break;
 988         case PE_WF_RECV_APPS:
 989                 current_parameter_apps(PT_WF_RECV, buf, sz);
 990                 break;
 991         case PE_WF_SEND_APPS:
 992                 current_parameter_apps(PT_WF_SEND, buf, sz);
 993                 break;
 994         case PE_L2CAP_RECV_APPS:
 995                 current_parameter_apps(PT_L2CAP_RECV, buf, sz);
 996                 break;
 997         case PE_SCO_RECV_APPS:
 998                 current_parameter_apps(PT_SCO_RECV, buf, sz);
 999                 break;
1000         case PT_SEND_ACL_APPS:
1001                 current_parameter_apps(PT_SEND_ACL, buf, sz);
1002                 break;
1003         case PT_SEND_SCO_APPS:
1004                 current_parameter_apps(PT_SEND_SCO, buf, sz);
1005                 break;
1006         default:
1007                 ret = -EINVAL;
1008                 break;
1009         }
1010
1011         return ret;
1012 }
1013
1014 int do_set_energy(void)
1015 {
1016         int ret = 0;
1017
1018         init_data_energy();
1019
1020         ret = swap_register_kretprobe(&sys_read_krp);
1021         if (ret) {
1022                 printk("swap_register_kretprobe(sys_read) result=%d!\n", ret);
1023                 return ret;
1024         }
1025
1026         ret = swap_register_kretprobe(&sys_write_krp);
1027         if (ret != 0) {
1028                 printk("swap_register_kretprobe(sys_write) result=%d!\n", ret);
1029                 goto unregister_sys_read;
1030         }
1031
1032         ret = swap_register_kretprobe(&switch_to_krp);
1033         if (ret) {
1034                 printk("swap_register_kretprobe(__switch_to) result=%d!\n",
1035                        ret);
1036                 goto unregister_sys_write;
1037         }
1038
1039         energy_xxx_set(bt_probes, bt_probes_cnt, &energy_bt_flag);
1040         energy_xxx_set(wifi_probes, wifi_probes_cnt, &wifi_flag);
1041
1042         /* TODO: check return value */
1043         lcd_set_energy();
1044
1045         return ret;
1046
1047 unregister_sys_read:
1048         swap_unregister_kretprobe(&sys_read_krp);
1049
1050 unregister_sys_write:
1051         swap_unregister_kretprobe(&sys_write_krp);
1052
1053         return ret;
1054 }
1055
1056 void do_unset_energy(void)
1057 {
1058         lcd_unset_energy();
1059         energy_xxx_unset(wifi_probes, wifi_probes_cnt, &wifi_flag);
1060         energy_xxx_unset(bt_probes, bt_probes_cnt, &energy_bt_flag);
1061
1062         swap_unregister_kretprobe(&switch_to_krp);
1063         swap_unregister_kretprobe(&sys_write_krp);
1064         swap_unregister_kretprobe(&sys_read_krp);
1065
1066         uninit_data_energy();
1067 }
1068
1069 static DEFINE_MUTEX(mutex_enable);
1070 static int energy_enable = 0;
1071
1072 /**
1073  * @brief Start measuring the energy consumption
1074  *
1075  * @return Error code
1076  */
1077 int set_energy(void)
1078 {
1079         int ret = -EINVAL;
1080
1081         mutex_lock(&mutex_enable);
1082         if (energy_enable) {
1083                 printk("energy profiling is already run!\n");
1084                 goto unlock;
1085         }
1086
1087         ret = do_set_energy();
1088         if (ret == 0)
1089                 energy_enable = 1;
1090
1091 unlock:
1092         mutex_unlock(&mutex_enable);
1093
1094         return ret;
1095 }
1096 EXPORT_SYMBOL_GPL(set_energy);
1097
1098 /**
1099  * @brief Stop measuring the energy consumption
1100  *
1101  * @return Error code
1102  */
1103 int unset_energy(void)
1104 {
1105         int ret = 0;
1106
1107         mutex_lock(&mutex_enable);
1108         if (energy_enable == 0) {
1109                 printk("energy profiling is not running!\n");
1110                 ret = -EINVAL;
1111                 goto unlock;
1112         }
1113
1114         do_unset_energy();
1115
1116         energy_enable = 0;
1117 unlock:
1118         mutex_unlock(&mutex_enable);
1119
1120         return ret;
1121 }
1122 EXPORT_SYMBOL_GPL(unset_energy);
1123
1124 int energy_once(void)
1125 {
1126         const char *sym;
1127
1128         sym = "__switch_to";
1129         switch_to_krp.kp.addr = (kprobe_opcode_t *)swap_ksyms(sym);
1130         if (switch_to_krp.kp.addr == NULL)
1131                 goto not_found;
1132
1133         sym = "sys_read";
1134         sys_read_krp.kp.addr = (kprobe_opcode_t *)swap_ksyms(sym);
1135         if (sys_read_krp.kp.addr == NULL)
1136                 goto not_found;
1137
1138         sym = "sys_write";
1139         sys_write_krp.kp.addr = (kprobe_opcode_t *)swap_ksyms(sym);
1140         if (sys_write_krp.kp.addr == NULL)
1141                 goto not_found;
1142
1143         energy_xxx_once(bt_probes, bt_probes_cnt);
1144         energy_xxx_once(wifi_probes, wifi_probes_cnt);
1145
1146         return 0;
1147
1148 not_found:
1149         printk("ERROR: symbol '%s' not found\n", sym);
1150         return -ESRCH;
1151 }
1152
1153 /**
1154  * @brief Initialization energy
1155  *
1156  * @return Error code
1157  */
1158 int energy_init(void)
1159 {
1160         int ret;
1161
1162         ret = init_feature();
1163         if (ret)
1164                 printk(KERN_INFO "Cannot init feature\n");
1165
1166         return ret;
1167 }
1168
1169 /**
1170  * @brief Deinitialization energy
1171  *
1172  * @return Void
1173  */
1174 void energy_uninit(void)
1175 {
1176         uninit_feature();
1177
1178         if (energy_enable)
1179                 do_unset_energy();
1180 }