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(KERN_INFO "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(KERN_INFO "XXX %s[%d/%d]: nnitializing tmstamp(%p) "
 186                        "on cpu(%d)\n",
 187                        current->comm, current->tgid, current->pid, tm, cpu);
 188                 tm_stat_set_timestamp(tm, start_time);
 189         }
 190
 191         tm_stat_update(tm, now);
 192         tm_stat_set_timestamp(tm, 0); /* set timestamp to 0 */
 193 }
 194
 195
 196
 197
 198
 199 struct energy_data {
 200         /* for __switch_to */
 201         struct cpus_time ct;
 202
 203         /* for sys_read */
 204         atomic64_t bytes_read;
 205
 206         /*for sys_write */
 207         atomic64_t bytes_written;
 208
 209         /*for recvmsg*/
 210         atomic64_t bytes_recv;
 211
 212         /* for sock_send */
 213         atomic64_t bytes_send;
 214
 215         /* for l2cap_recv */
 216         atomic64_t bytes_l2cap_recv_acldata;
 217
 218         /* for sco_recv_scodata */
 219         atomic64_t bytes_sco_recv_scodata;
 220
 221         /* for hci_send_acl */
 222         atomic64_t bytes_hci_send_acl;
 223
 224         /* for hci_send_sco */
 225         atomic64_t bytes_hci_send_sco;
 226 };
 227
 228 static sspt_feature_id_t feature_id = SSPT_FEATURE_ID_BAD;
 229
 230 static void init_ed(struct energy_data *ed)
 231 {
 232         /* instead of get_ntime(), CPU time is initialized to 0 here. Timestamp
 233          * value will be properly set when the corresponding __switch_to event
 234          * occurs */
 235         cpus_time_init(&ed->ct, 0);
 236         atomic64_set(&ed->bytes_read, 0);
 237         atomic64_set(&ed->bytes_written, 0);
 238         atomic64_set(&ed->bytes_recv, 0);
 239         atomic64_set(&ed->bytes_send, 0);
 240         atomic64_set(&ed->bytes_l2cap_recv_acldata, 0);
 241         atomic64_set(&ed->bytes_sco_recv_scodata, 0);
 242         atomic64_set(&ed->bytes_hci_send_acl, 0);
 243         atomic64_set(&ed->bytes_hci_send_sco, 0);
 244 }
 245
 246 static void uninit_ed(struct energy_data *ed)
 247 {
 248         cpus_time_init(&ed->ct, 0);
 249         atomic64_set(&ed->bytes_read, 0);
 250         atomic64_set(&ed->bytes_written, 0);
 251         atomic64_set(&ed->bytes_recv, 0);
 252         atomic64_set(&ed->bytes_send, 0);
 253         atomic64_set(&ed->bytes_l2cap_recv_acldata, 0);
 254         atomic64_set(&ed->bytes_sco_recv_scodata, 0);
 255         atomic64_set(&ed->bytes_hci_send_acl, 0);
 256         atomic64_set(&ed->bytes_hci_send_sco, 0);
 257 }
 258
 259 static void *create_ed(void)
 260 {
 261         struct energy_data *ed;
 262
 263         ed = kmalloc(sizeof(*ed), GFP_ATOMIC);
 264         if (ed)
 265                 init_ed(ed);
 266
 267         return (void *)ed;
 268 }
 269
 270 static void destroy_ed(void *data)
 271 {
 272         struct energy_data *ed = (struct energy_data *)data;
 273         kfree(ed);
 274 }
 275
 276
 277 static int init_feature(void)
 278 {
 279         feature_id = sspt_register_feature(create_ed, destroy_ed);
 280
 281         if (feature_id == SSPT_FEATURE_ID_BAD)
 282                 return -EPERM;
 283
 284         return 0;
 285 }
 286
 287 static void uninit_feature(void)
 288 {
 289         sspt_unregister_feature(feature_id);
 290         feature_id = SSPT_FEATURE_ID_BAD;
 291 }
 292
 293 static struct energy_data *get_energy_data(struct task_struct *task)
 294 {
 295         void *data = NULL;
 296         struct sspt_proc *proc;
 297
 298         proc = sspt_proc_by_task(task);
 299         if (proc)
 300                 data = sspt_get_feature_data(proc->feature, feature_id);
 301
 302         return (struct energy_data *)data;
 303 }
 304
 305 static int check_fs(unsigned long magic)
 306 {
 307         switch (magic) {
 308         case EXT2_SUPER_MAGIC: /* == EXT3_SUPER_MAGIC == EXT4_SUPER_MAGIC */
 309         case MSDOS_SUPER_MAGIC:
 310                 return 1;
 311         }
 312
 313         return 0;
 314 }
 315
 316 static int check_ftype(int fd)
 317 {
 318         int err, ret = 0;
 319         struct kstat kstat;
 320
 321         err = vfs_fstat(fd, &kstat);
 322         if (err == 0 && S_ISREG(kstat.mode))
 323                 ret = 1;
 324
 325         return ret;
 326 }
 327
 328 static int check_file(int fd)
 329 {
 330         struct file *file;
 331
 332         file = fget(fd);
 333         if (file) {
 334                 int magic = 0;
 335                 if (file->f_path.dentry && file->f_path.dentry->d_sb)
 336                         magic = file->f_path.dentry->d_sb->s_magic;
 337
 338                 fput(file);
 339
 340                 if (check_fs(magic) && check_ftype(fd))
 341                         return 1;
 342         }
 343
 344         return 0;
 345 }
 346
 347 static unsigned long get_arg0(struct pt_regs *regs)
 348 {
 349 #if defined(CONFIG_ARM)
 350         return regs->ARM_r0;
 351 #elif defined(CONFIG_X86_32)
 352         return regs->bx;
 353 #else
 354         #error "this architecture is not supported"
 355 #endif /* CONFIG_arch */
 356 }
 357
 358
 359
 360
 361
 362 static struct cpus_time ct_idle;
 363 static struct energy_data ed_system;
 364 static u64 start_time;
 365
 366 static void init_data_energy(void)
 367 {
 368         start_time = get_ntime();
 369         init_ed(&ed_system);
 370         cpus_time_init(&ct_idle, 0);
 371 }
 372
 373 static void uninit_data_energy(void)
 374 {
 375         start_time = 0;
 376         uninit_ed(&ed_system);
 377         cpus_time_init(&ct_idle, 0);
 378 }
 379
 380
 381
 382
 383
 384 /* ============================================================================
 385  * =                             __switch_to                                  =
 386  * ============================================================================
 387  */
 388 static int entry_handler_switch(struct kretprobe_instance *ri,
 389                                 struct pt_regs *regs)
 390 {
 391         int cpu;
 392         struct cpus_time *ct;
 393         struct energy_data *ed;
 394         unsigned long flags;
 395
 396         cpu = smp_processor_id();
 397
 398         ct = current->tgid ? &ed_system.ct : &ct_idle;
 399         cpus_time_lock(ct, flags);
 400         cpus_time_update_running(ct, cpu, get_ntime(), start_time);
 401         cpus_time_unlock(ct, flags);
 402
 403         ed = get_energy_data(current);
 404         if (ed) {
 405                 ct = &ed->ct;
 406                 cpus_time_lock(ct, flags);
 407                 cpus_time_update_running(ct, cpu, get_ntime(), start_time);
 408                 cpus_time_unlock(ct, flags);
 409         }
 410
 411         return 0;
 412 }
 413
 414 static int ret_handler_switch(struct kretprobe_instance *ri,
 415                               struct pt_regs *regs)
 416 {
 417         int cpu;
 418         struct cpus_time *ct;
 419         struct energy_data *ed;
 420         unsigned long flags;
 421
 422         cpu = smp_processor_id();
 423
 424         ct = current->tgid ? &ed_system.ct : &ct_idle;
 425         cpus_time_lock(ct, flags);
 426         cpus_time_save_entry(ct, cpu, get_ntime());
 427         cpus_time_unlock(ct, flags);
 428
 429         ed = get_energy_data(current);
 430         if (ed) {
 431                 ct = &ed->ct;
 432                 cpus_time_lock(ct, flags);
 433                 cpus_time_save_entry(ct, cpu, get_ntime());
 434                 cpus_time_unlock(ct, flags);
 435         }
 436
 437         return 0;
 438 }
 439
 440 static struct kretprobe switch_to_krp = {
 441         .entry_handler = entry_handler_switch,
 442         .handler = ret_handler_switch,
 443 };
 444
 445
 446
 447
 448
 449 /* ============================================================================
 450  * =                                sys_read                                  =
 451  * ============================================================================
 452  */
 453 struct sys_read_data {
 454         int fd;
 455 };
 456
 457 static int entry_handler_sys_read(struct kretprobe_instance *ri,
 458                                   struct pt_regs *regs)
 459 {
 460         struct sys_read_data *srd = (struct sys_read_data *)ri->data;
 461
 462         srd->fd = (int)get_arg0(regs);
 463
 464         return 0;
 465 }
 466
 467 static int ret_handler_sys_read(struct kretprobe_instance *ri,
 468                                 struct pt_regs *regs)
 469 {
 470         int ret = regs_return_value(regs);
 471
 472         if (ret > 0) {
 473                 struct sys_read_data *srd;
 474
 475                 srd = (struct sys_read_data *)ri->data;
 476                 if (check_file(srd->fd)) {
 477                         struct energy_data *ed;
 478
 479                         ed = get_energy_data(current);
 480                         if (ed)
 481                                 atomic64_add(ret, &ed->bytes_read);
 482
 483                         atomic64_add(ret, &ed_system.bytes_read);
 484                 }
 485         }
 486
 487         return 0;
 488 }
 489
 490 static struct kretprobe sys_read_krp = {
 491         .entry_handler = entry_handler_sys_read,
 492         .handler = ret_handler_sys_read,
 493         .data_size = sizeof(struct sys_read_data)
 494 };
 495
 496
 497
 498
 499
 500 /* ============================================================================
 501  * =                               sys_write                                  =
 502  * ============================================================================
 503  */
 504 static int entry_handler_sys_write(struct kretprobe_instance *ri,
 505                                    struct pt_regs *regs)
 506 {
 507         struct sys_read_data *srd = (struct sys_read_data *)ri->data;
 508
 509         srd->fd = (int)get_arg0(regs);
 510
 511         return 0;
 512 }
 513
 514 static int ret_handler_sys_write(struct kretprobe_instance *ri,
 515                                  struct pt_regs *regs)
 516 {
 517         int ret = regs_return_value(regs);
 518
 519         if (ret > 0) {
 520                 struct sys_read_data *srd;
 521
 522                 srd = (struct sys_read_data *)ri->data;
 523                 if (check_file(srd->fd)) {
 524                         struct energy_data *ed;
 525
 526                         ed = get_energy_data(current);
 527                         if (ed)
 528                                 atomic64_add(ret, &ed->bytes_written);
 529
 530                         atomic64_add(ret, &ed_system.bytes_written);
 531                 }
 532         }
 533
 534         return 0;
 535 }
 536
 537 static struct kretprobe sys_write_krp = {
 538         .entry_handler = entry_handler_sys_write,
 539         .handler = ret_handler_sys_write,
 540         .data_size = sizeof(struct sys_read_data)
 541 };
 542
 543
 544
 545
 546
 547 /* ============================================================================
 548  * =                                wifi                                      =
 549  * ============================================================================
 550  */
 551 static bool check_wlan0(struct socket *sock)
 552 {
 553         /* FIXME: hardcode interface */
 554         const char *name_intrf = "wlan0";
 555
 556         if (sock->sk->sk_dst_cache &&
 557             sock->sk->sk_dst_cache->dev &&
 558             !strcmp(sock->sk->sk_dst_cache->dev->name, name_intrf))
 559                 return true;
 560
 561         return false;
 562 }
 563
 564 static bool check_socket(struct task_struct *task, struct socket *socket)
 565 {
 566         bool ret = false;
 567         unsigned int fd;
 568         struct files_struct *files;
 569
 570         files = swap_get_files_struct(task);
 571         if (files == NULL)
 572                 return false;
 573
 574         rcu_read_lock();
 575         for (fd = 0; fd < files_fdtable(files)->max_fds; ++fd) {
 576                 if (fcheck_files(files, fd) == socket->file) {
 577                         ret = true;
 578                         goto unlock;
 579                 }
 580         }
 581
 582 unlock:
 583         rcu_read_unlock();
 584         swap_put_files_struct(files);
 585         return ret;
 586 }
 587
 588 static struct energy_data *get_energy_data_by_socket(struct task_struct *task,
 589                                                      struct socket *socket)
 590 {
 591         struct energy_data *ed;
 592
 593         ed = get_energy_data(task);
 594         if (ed)
 595                 ed = check_socket(task, socket) ? ed : NULL;
 596
 597         return ed;
 598 }
 599
 600 static int wf_sock_eh(struct kretprobe_instance *ri, struct pt_regs *regs)
 601 {
 602         struct socket *socket = (struct socket *)swap_get_karg(regs, 0);
 603
 604         *(struct socket **)ri->data = check_wlan0(socket) ? socket : NULL;
 605
 606         return 0;
 607 }
 608
 609 static int wf_sock_aio_eh(struct kretprobe_instance *ri, struct pt_regs *regs)
 610 {
 611         struct kiocb *iocb = (struct kiocb *)swap_get_karg(regs, 0);
 612         struct socket *socket = iocb->ki_filp->private_data;
 613
 614         *(struct socket **)ri->data = check_wlan0(socket) ? socket : NULL;
 615
 616         return 0;
 617 }
 618
 619 static int wf_sock_recv_rh(struct kretprobe_instance *ri, struct pt_regs *regs)
 620 {
 621         int ret = regs_return_value(regs);
 622
 623         if (ret > 0) {
 624                 struct socket *socket = *(struct socket **)ri->data;
 625
 626                 if (socket) {
 627                         struct energy_data *ed;
 628
 629                         ed = get_energy_data_by_socket(current, socket);
 630                         if (ed)
 631                                 atomic64_add(ret, &ed->bytes_recv);
 632                         atomic64_add(ret, &ed_system.bytes_recv);
 633                 }
 634         }
 635
 636         return 0;
 637 }
 638
 639 static int wf_sock_send_rh(struct kretprobe_instance *ri, struct pt_regs *regs)
 640 {
 641         int ret = regs_return_value(regs);
 642
 643         if (ret > 0) {
 644                 struct socket *socket = *(struct socket **)ri->data;
 645
 646                 if (socket) {
 647                         struct energy_data *ed;
 648
 649                         ed = get_energy_data_by_socket(current, socket);
 650                         if (ed)
 651                                 atomic64_add(ret, &ed->bytes_send);
 652                         atomic64_add(ret, &ed_system.bytes_send);
 653                 }
 654         }
 655
 656         return 0;
 657 }
 658
 659 static struct kretprobe sock_recv_krp = {
 660         .entry_handler = wf_sock_eh,
 661         .handler = wf_sock_recv_rh,
 662         .data_size = sizeof(struct socket *)
 663 };
 664
 665 static struct kretprobe sock_send_krp = {
 666         .entry_handler = wf_sock_eh,
 667         .handler = wf_sock_send_rh,
 668         .data_size = sizeof(struct socket *)
 669 };
 670
 671 static struct kretprobe sock_aio_read_krp = {
 672         .entry_handler = wf_sock_aio_eh,
 673         .handler = wf_sock_recv_rh,
 674         .data_size = sizeof(struct socket *)
 675 };
 676
 677 static struct kretprobe sock_aio_write_krp = {
 678         .entry_handler = wf_sock_aio_eh,
 679         .handler = wf_sock_send_rh,
 680         .data_size = sizeof(struct socket *)
 681 };
 682
 683 static struct kern_probe wifi_probes[] = {
 684         {
 685                 .name = "sock_recvmsg",
 686                 .rp = &sock_recv_krp,
 687         },
 688         {
 689                 .name = "sock_sendmsg",
 690                 .rp = &sock_send_krp,
 691         },
 692         {
 693                 .name = "sock_aio_read",
 694                 .rp = &sock_aio_read_krp,
 695         },
 696         {
 697                 .name = "sock_aio_write",
 698                 .rp = &sock_aio_write_krp,
 699         }
 700 };
 701
 702 enum { wifi_probes_cnt = ARRAY_SIZE(wifi_probes) };
 703 static int wifi_flag = 0;
 704
 705
 706
 707
 708
 709 /* ============================================================================
 710  * =                                bluetooth                                 =
 711  * ============================================================================
 712  */
 713
 714 struct swap_bt_data {
 715         struct socket *socket;
 716 };
 717
 718 static int bt_entry_handler(struct kretprobe_instance *ri,
 719                             struct pt_regs *regs)
 720 {
 721         struct swap_bt_data *data = (struct swap_bt_data *)ri->data;
 722         struct socket *sock = (struct socket *)swap_get_sarg(regs, 1);
 723
 724         data->socket = sock ? sock : NULL;
 725
 726         return 0;
 727 }
 728
 729 static int bt_recvmsg_handler(struct kretprobe_instance *ri,
 730                               struct pt_regs *regs)
 731 {
 732         int ret = regs_return_value(regs);
 733         struct swap_bt_data *data = (struct swap_bt_data *)ri->data;
 734
 735         if (ret > 0) {
 736                 struct socket *socket = data->socket;
 737
 738                 if (socket) {
 739                         struct energy_data *ed;
 740
 741                         ed = get_energy_data_by_socket(current, socket);
 742                         if (ed)
 743                                 atomic64_add(ret, &ed->bytes_l2cap_recv_acldata);
 744                 }
 745                 atomic64_add(ret, &ed_system.bytes_l2cap_recv_acldata);
 746         }
 747
 748         return 0;
 749 }
 750
 751 static int bt_sendmsg_handler(struct kretprobe_instance *ri,
 752                               struct pt_regs *regs)
 753 {
 754         int ret = regs_return_value(regs);
 755         struct swap_bt_data *data = (struct swap_bt_data *)ri->data;
 756
 757         if (ret > 0) {
 758                 struct socket *socket = data->socket;
 759
 760                 if (socket) {
 761                         struct energy_data *ed;
 762
 763                         ed = get_energy_data_by_socket(current, socket);
 764                         if (ed)
 765                                 atomic64_add(ret, &ed->bytes_hci_send_sco);
 766                 }
 767                 atomic64_add(ret, &ed_system.bytes_hci_send_sco);
 768         }
 769
 770         return 0;
 771 }
 772
 773 static struct kretprobe rfcomm_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 l2cap_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 hci_sock_recvmsg_krp = {
 786         .entry_handler = bt_entry_handler,
 787         .handler = bt_recvmsg_handler,
 788         .data_size = sizeof(struct swap_bt_data)
 789 };
 790
 791 static struct kretprobe sco_sock_recvmsg_krp = {
 792         .entry_handler = bt_entry_handler,
 793         .handler = bt_recvmsg_handler,
 794         .data_size = sizeof(struct swap_bt_data)
 795 };
 796 static struct kretprobe rfcomm_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 l2cap_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 hci_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 kretprobe sco_sock_sendmsg_krp = {
 815         .entry_handler = bt_entry_handler,
 816         .handler = bt_sendmsg_handler,
 817         .data_size = sizeof(struct swap_bt_data)
 818 };
 819
 820 static struct kern_probe bt_probes[] = {
 821         {
 822                 .name = "rfcomm_sock_recvmsg",
 823                 .rp = &rfcomm_sock_recvmsg_krp,
 824         },
 825         {
 826                 .name = "l2cap_sock_recvmsg",
 827                 .rp = &l2cap_sock_recvmsg_krp,
 828         },
 829         {
 830                 .name = "hci_sock_recvmsg",
 831                 .rp = &hci_sock_recvmsg_krp,
 832         },
 833         {
 834                 .name = "sco_sock_recvmsg",
 835                 .rp = &sco_sock_recvmsg_krp,
 836         },
 837         {
 838                 .name = "rfcomm_sock_sendmsg",
 839                 .rp = &rfcomm_sock_sendmsg_krp,
 840         },
 841         {
 842                 .name = "l2cap_sock_sendmsg",
 843                 .rp = &l2cap_sock_sendmsg_krp,
 844         },
 845         {
 846                 .name = "hci_sock_sendmsg",
 847                 .rp = &hci_sock_sendmsg_krp,
 848         },
 849         {
 850                 .name = "sco_sock_sendmsg",
 851                 .rp = &sco_sock_sendmsg_krp,
 852         }
 853 };
 854
 855 enum { bt_probes_cnt = ARRAY_SIZE(bt_probes) };
 856 static int energy_bt_flag = 0;
 857
 858 enum parameter_type {
 859         PT_CPU,
 860         PT_READ,
 861         PT_WRITE,
 862         PT_WF_RECV,
 863         PT_WF_SEND,
 864         PT_L2CAP_RECV,
 865         PT_SCO_RECV,
 866         PT_SEND_ACL,
 867         PT_SEND_SCO
 868 };
 869
 870 struct cmd_pt {
 871         enum parameter_type pt;
 872         void *buf;
 873         int sz;
 874 };
 875
 876 static void callback_for_proc(struct sspt_proc *proc, void *data)
 877 {
 878         void *f_data = sspt_get_feature_data(proc->feature, feature_id);
 879         struct energy_data *ed = (struct energy_data *)f_data;
 880
 881         if (ed) {
 882                 unsigned long flags;
 883                 struct cmd_pt *cmdp = (struct cmd_pt *)data;
 884                 u64 *val = cmdp->buf;
 885
 886                 switch (cmdp->pt) {
 887                 case PT_CPU:
 888                         cpus_time_lock(&ed->ct, flags);
 889                         cpus_time_sum_running_all(&ed->ct, val, get_ntime());
 890                         cpus_time_unlock(&ed->ct, flags);
 891                         break;
 892                 case PT_READ:
 893                         *val += atomic64_read(&ed->bytes_read);
 894                         break;
 895                 case PT_WRITE:
 896                         *val += atomic64_read(&ed->bytes_written);
 897                         break;
 898                 case PT_WF_RECV:
 899                         *val += atomic64_read(&ed->bytes_recv);
 900                         break;
 901                 case PT_WF_SEND:
 902                         *val += atomic64_read(&ed->bytes_send);
 903                         break;
 904                 case PT_L2CAP_RECV:
 905                         *val += atomic64_read(&ed->bytes_l2cap_recv_acldata);
 906                         break;
 907                 case PT_SCO_RECV:
 908                         *val += atomic64_read(&ed->bytes_sco_recv_scodata);
 909                         break;
 910                 case PT_SEND_ACL:
 911                         *val += atomic64_read(&ed->bytes_hci_send_acl);
 912                         break;
 913                 case PT_SEND_SCO:
 914                         *val += atomic64_read(&ed->bytes_hci_send_sco);
 915                         break;
 916                 default:
 917                         break;
 918                 }
 919         }
 920 }
 921
 922 static int current_parameter_apps(enum parameter_type pt, void *buf, int sz)
 923 {
 924         struct cmd_pt cmdp;
 925
 926         cmdp.pt = pt;
 927         cmdp.buf = buf;
 928         cmdp.sz = sz;
 929
 930         on_each_proc(callback_for_proc, (void *)&cmdp);
 931
 932         return 0;
 933 }
 934
 935 /**
 936  * @brief Get energy parameter
 937  *
 938  * @param pe Type of energy parameter
 939  * @param buf Buffer
 940  * @param sz Buffer size
 941  * @return Error code
 942  */
 943 int get_parameter_energy(enum parameter_energy pe, void *buf, size_t sz)
 944 {
 945         unsigned long flags;
 946         u64 *val = buf; /* currently all parameters are u64 vals */
 947         int ret = 0;
 948
 949         switch (pe) {
 950         case PE_TIME_IDLE:
 951                 cpus_time_lock(&ct_idle, flags);
 952                 /* for the moment we consider only CPU[0] idle time */
 953                 *val = cpu_time_get_running(&ct_idle, 0, get_ntime());
 954                 cpus_time_unlock(&ct_idle, flags);
 955                 break;
 956         case PE_TIME_SYSTEM:
 957                 cpus_time_lock(&ed_system.ct, flags);
 958                 cpus_time_get_running_all(&ed_system.ct, val, get_ntime());
 959                 cpus_time_unlock(&ed_system.ct, flags);
 960                 break;
 961         case PE_TIME_APPS:
 962                 current_parameter_apps(PT_CPU, buf, sz);
 963                 break;
 964         case PE_READ_SYSTEM:
 965                 *val = atomic64_read(&ed_system.bytes_read);
 966                 break;
 967         case PE_WRITE_SYSTEM:
 968                 *val = atomic64_read(&ed_system.bytes_written);
 969                 break;
 970         case PE_WF_RECV_SYSTEM:
 971                 *val = atomic64_read(&ed_system.bytes_recv);
 972                 break;
 973         case PE_WF_SEND_SYSTEM:
 974                 *val = atomic64_read(&ed_system.bytes_send);
 975                 break;
 976         case PE_L2CAP_RECV_SYSTEM:
 977                 *val = atomic64_read(&ed_system.bytes_l2cap_recv_acldata);
 978                 break;
 979         case PE_SCO_RECV_SYSTEM:
 980                 *val = atomic64_read(&ed_system.bytes_sco_recv_scodata);
 981                 break;
 982         case PT_SEND_ACL_SYSTEM:
 983                 *val = atomic64_read(&ed_system.bytes_hci_send_acl);
 984                 break;
 985         case PT_SEND_SCO_SYSTEM:
 986                 *val = atomic64_read(&ed_system.bytes_hci_send_sco);
 987                 break;
 988         case PE_READ_APPS:
 989                 current_parameter_apps(PT_READ, buf, sz);
 990                 break;
 991         case PE_WRITE_APPS:
 992                 current_parameter_apps(PT_WRITE, buf, sz);
 993                 break;
 994         case PE_WF_RECV_APPS:
 995                 current_parameter_apps(PT_WF_RECV, buf, sz);
 996                 break;
 997         case PE_WF_SEND_APPS:
 998                 current_parameter_apps(PT_WF_SEND, buf, sz);
 999                 break;
1000         case PE_L2CAP_RECV_APPS:
1001                 current_parameter_apps(PT_L2CAP_RECV, buf, sz);
1002                 break;
1003         case PE_SCO_RECV_APPS:
1004                 current_parameter_apps(PT_SCO_RECV, buf, sz);
1005                 break;
1006         case PT_SEND_ACL_APPS:
1007                 current_parameter_apps(PT_SEND_ACL, buf, sz);
1008                 break;
1009         case PT_SEND_SCO_APPS:
1010                 current_parameter_apps(PT_SEND_SCO, buf, sz);
1011                 break;
1012         default:
1013                 ret = -EINVAL;
1014                 break;
1015         }
1016
1017         return ret;
1018 }
1019
1020 int do_set_energy(void)
1021 {
1022         int ret = 0;
1023
1024         init_data_energy();
1025
1026         ret = swap_register_kretprobe(&sys_read_krp);
1027         if (ret) {
1028                 printk(KERN_INFO "swap_register_kretprobe(sys_read) "
1029                        "result=%d!\n", ret);
1030                 return ret;
1031         }
1032
1033         ret = swap_register_kretprobe(&sys_write_krp);
1034         if (ret != 0) {
1035                 printk(KERN_INFO "swap_register_kretprobe(sys_write) "
1036                        "result=%d!\n", ret);
1037                 goto unregister_sys_read;
1038         }
1039
1040         ret = swap_register_kretprobe(&switch_to_krp);
1041         if (ret) {
1042                 printk(KERN_INFO "swap_register_kretprobe(__switch_to) "
1043                        "result=%d!\n",
1044                        ret);
1045                 goto unregister_sys_write;
1046         }
1047
1048         energy_xxx_set(bt_probes, bt_probes_cnt, &energy_bt_flag);
1049         energy_xxx_set(wifi_probes, wifi_probes_cnt, &wifi_flag);
1050
1051         /* TODO: check return value */
1052         lcd_set_energy();
1053
1054         return ret;
1055
1056 unregister_sys_read:
1057         swap_unregister_kretprobe(&sys_read_krp);
1058
1059 unregister_sys_write:
1060         swap_unregister_kretprobe(&sys_write_krp);
1061
1062         return ret;
1063 }
1064
1065 void do_unset_energy(void)
1066 {
1067         lcd_unset_energy();
1068         energy_xxx_unset(wifi_probes, wifi_probes_cnt, &wifi_flag);
1069         energy_xxx_unset(bt_probes, bt_probes_cnt, &energy_bt_flag);
1070
1071         swap_unregister_kretprobe(&switch_to_krp);
1072         swap_unregister_kretprobe(&sys_write_krp);
1073         swap_unregister_kretprobe(&sys_read_krp);
1074
1075         uninit_data_energy();
1076 }
1077
1078 static DEFINE_MUTEX(mutex_enable);
1079 static int energy_enable;
1080
1081 /**
1082  * @brief Start measuring the energy consumption
1083  *
1084  * @return Error code
1085  */
1086 int set_energy(void)
1087 {
1088         int ret = -EINVAL;
1089
1090         mutex_lock(&mutex_enable);
1091         if (energy_enable) {
1092                 printk(KERN_INFO "energy profiling is already run!\n");
1093                 goto unlock;
1094         }
1095
1096         ret = do_set_energy();
1097         if (ret == 0)
1098                 energy_enable = 1;
1099
1100 unlock:
1101         mutex_unlock(&mutex_enable);
1102
1103         return ret;
1104 }
1105 EXPORT_SYMBOL_GPL(set_energy);
1106
1107 /**
1108  * @brief Stop measuring the energy consumption
1109  *
1110  * @return Error code
1111  */
1112 int unset_energy(void)
1113 {
1114         int ret = 0;
1115
1116         mutex_lock(&mutex_enable);
1117         if (energy_enable == 0) {
1118                 printk(KERN_INFO "energy profiling is not running!\n");
1119                 ret = -EINVAL;
1120                 goto unlock;
1121         }
1122
1123         do_unset_energy();
1124
1125         energy_enable = 0;
1126 unlock:
1127         mutex_unlock(&mutex_enable);
1128
1129         return ret;
1130 }
1131 EXPORT_SYMBOL_GPL(unset_energy);
1132
1133 int energy_once(void)
1134 {
1135         const char *sym;
1136
1137         sym = "__switch_to";
1138         switch_to_krp.kp.addr = (kprobe_opcode_t *)swap_ksyms(sym);
1139         if (switch_to_krp.kp.addr == NULL)
1140                 goto not_found;
1141
1142         sym = "sys_read";
1143         sys_read_krp.kp.addr = (kprobe_opcode_t *)swap_ksyms(sym);
1144         if (sys_read_krp.kp.addr == NULL)
1145                 goto not_found;
1146
1147         sym = "sys_write";
1148         sys_write_krp.kp.addr = (kprobe_opcode_t *)swap_ksyms(sym);
1149         if (sys_write_krp.kp.addr == NULL)
1150                 goto not_found;
1151
1152         energy_xxx_once(bt_probes, bt_probes_cnt);
1153         energy_xxx_once(wifi_probes, wifi_probes_cnt);
1154
1155         return 0;
1156
1157 not_found:
1158         printk(KERN_INFO "ERROR: symbol '%s' not found\n", sym);
1159         return -ESRCH;
1160 }
1161
1162 /**
1163  * @brief Initialization energy
1164  *
1165  * @return Error code
1166  */
1167 int energy_init(void)
1168 {
1169         int ret;
1170
1171         ret = init_feature();
1172         if (ret)
1173                 printk(KERN_INFO "Cannot init feature\n");
1174
1175         return ret;
1176 }
1177
1178 /**
1179  * @brief Deinitialization energy
1180  *
1181  * @return Void
1182  */
1183 void energy_uninit(void)
1184 {
1185         uninit_feature();
1186
1187         if (energy_enable)
1188                 do_unset_energy();
1189 }