← Back to branch summary

~pali/+junk/maemo_libbqioctl

« back to all changes in this revision

Viewing changes to libbqioctl.c

  • Committer: Pali Rohár
  • Date: 2011-09-21 11:54:42 UTC
  • Revision ID: pali.rohar@gmail.com-20110921115442-ukoppqdw17emhzmm
cleanup, add debug support

Show diffs side-by-side

added added

removed removed

Lines of Context:
libbqioctl.c
1
 
#include <string.h>
2
1
#include <stdio.h>
3
2
#include <stdlib.h>
4
 
 
5
 
//#include <sys/types.h>
6
 
//#include <sys/stat.h>
7
 
//#include <sys/ioctl.h>
8
 
//#include <fcntl.h>
 
3
#include <string.h>
 
4
#include <errno.h>
9
5
#include <dlfcn.h>
10
6
 
11
7
#define __u8 unsigned char
68
64
static int slave_addr[512];
69
65
static int (*open_libc)(const char *, int, mode_t);
70
66
static int (*close_libc)(int);
71
 
static int (*ioctl_libc)(int, int, ...);
 
67
static int (*ioctl_libc)(int, int, void *);
72
68
 
73
69
__attribute__((constructor)) static void init() {
74
70
 
75
 
        printf("init\n");
76
 
 
77
71
        void * handle;
78
72
        char * error;
 
73
        FILE * file;
 
74
 
 
75
#ifdef DEBUG
 
76
        fprintf(stderr, "libbqioctl: init\n");
 
77
#endif
 
78
 
 
79
        file = fopen("/sys/class/power_supply/bq27200-0/uevent", "r");
 
80
 
 
81
        if ( ! file ) {
 
82
 
 
83
                fprintf(stderr, "libbqioctl: /sys/class/power_supply/bq27200-0/uevent: cannot open file: %s\n", strerror(errno));
 
84
                fprintf(stderr, "libbqioctl: is kernel module bq27x00_battery loaded?\n");
 
85
                exit(EXIT_FAILURE);
 
86
 
 
87
        }
 
88
 
 
89
        fclose(file);
79
90
 
80
91
        handle = dlopen("libc.so.6", RTLD_LOCAL | RTLD_LAZY);
81
92
 
82
93
        if ( ! handle ) {
83
94
 
84
 
                fprintf(stderr, "%s\n", dlerror());
 
95
                fprintf(stderr, "libbqioctl: %s\n", dlerror());
85
96
                exit(EXIT_FAILURE);
86
97
 
87
98
        }
88
99
 
89
100
        dlerror();
90
101
 
91
 
        //*(void **) (&open_libc) = dlsym(handle, "open");
92
102
        open_libc = dlsym(handle, "open");
93
103
 
94
 
        if ( ( error = dlerror() ) != NULL ) {
 
104
        if ( ( error = dlerror() ) || ! open_libc ) {
95
105
 
96
 
                fprintf(stderr, "%s\n", dlerror());
 
106
                fprintf(stderr, "libbqioctl: %s\n", error);
97
107
                exit(EXIT_FAILURE);
98
108
 
99
109
        }
100
110
 
101
 
        //*(void **) (&close_libc) = dlsym(handle, "close");
102
111
        close_libc = dlsym(handle, "close");
103
112
 
104
 
        if ( ( error = dlerror() ) != NULL ) {
 
113
        if ( ( error = dlerror() ) || ! close_libc ) {
105
114
 
106
 
                fprintf(stderr, "%s\n", dlerror());
 
115
                fprintf(stderr, "libbqioctl: %s\n", error);
107
116
                exit(EXIT_FAILURE);
108
117
 
109
118
        }
110
119
 
111
 
        //*(void **) (&ioctl_libc) = dlsym(handle, "ioctl");
112
120
        ioctl_libc = dlsym(handle, "ioctl");
113
121
 
114
 
        if ( ( error = dlerror() ) != NULL ) {
 
122
        if ( ( error = dlerror() ) || ! ioctl_libc ) {
115
123
 
116
 
                fprintf(stderr, "%s\n", dlerror());
 
124
                fprintf(stderr, "libbqioctl: %s\n", error);
117
125
                exit(EXIT_FAILURE);
118
126
 
119
127
        }
124
132
 
125
133
int open(const char * pathname, int flags, mode_t mode) {
126
134
 
127
 
        printf("open: %s %d\n", pathname, flags);
128
 
 
129
 
        int ret = open_libc(pathname, flags, mode);
130
 
 
131
 
        if ( strcmp(pathname, "/dev/i2c-2") == 0 && ret >= 0 && ret < (int)sizeof(slave_addr) )
 
135
        int ret;
 
136
 
 
137
#ifdef DEBUG
 
138
        fprintf(stderr, "libbqioctl: open(\"%s\", %d, 0x%x)\n", pathname, flags, mode);
 
139
#endif
 
140
 
 
141
        ret = open_libc(pathname, flags, mode);
 
142
 
 
143
        if ( strcmp(pathname, "/dev/i2c-2") == 0 && ret >= 0 && ret < (int)sizeof(slave_addr) ) {
 
144
 
 
145
#ifdef DEBUG
 
146
                fprintf(stderr, "libbqioctl: register slave address for fd %d\n", ret);
 
147
#endif
 
148
 
132
149
                slave_addr[ret] = 0;
133
150
 
 
151
        }
 
152
 
134
153
        return ret;
135
154
 
136
155
}
137
156
 
138
157
int close(int fd) {
139
158
 
140
 
        printf("close: %d\n", fd);
141
 
 
142
 
        int ret = close_libc(fd);
143
 
 
144
 
        if ( ret == 0 && fd >= 0 && fd <= (int)sizeof(slave_addr) && slave_addr[fd] >= 0 )
 
159
        int ret;
 
160
 
 
161
#ifdef DEBUG
 
162
        fprintf(stderr, "libbqioctl: close(%d)\n", fd);
 
163
#endif
 
164
 
 
165
        ret = close_libc(fd);
 
166
 
 
167
        if ( ret == 0 && fd >= 0 && fd <= (int)sizeof(slave_addr) && slave_addr[fd] >= 0 ) {
 
168
 
 
169
#ifdef DEBUG
 
170
                fprintf(stderr, "libbqioctl: unregister slave address for fd %d\n", fd);
 
171
#endif
 
172
 
145
173
                slave_addr[fd] = -1;
146
174
 
 
175
        }
 
176
 
147
177
        return ret;
148
178
 
149
179
}
155
185
 
156
186
        if ( ! file ) {
157
187
 
 
188
                fprintf(stderr, "libbqioctl: %s: cannot open file: %s\n", path, strerror(errno));
 
189
                fprintf(stderr, "libbqioctl: is kernel module bq27x00_battery loaded?\n");
 
190
 
158
191
                *ok = 0;
159
192
                return 0;
160
193
 
174
207
        if ( reg == 0x06 ) // TEMP
175
208
                return ( bq_read_value("/sys/class/power_supply/bq27200-0/temp", ok) * 2 + 5463 ) / 5;
176
209
        else if ( reg == 0x08 ) // VOLT
177
 
                return bq_read_value("/sys/class/power_supply/bq27200-0/voltage_now", ok);
 
210
                return bq_read_value("/sys/class/power_supply/bq27200-0/voltage_now", ok) / 1000;
178
211
        else if ( reg == 0x14 ) // AI
179
 
                return bq_read_value("/sys/class/power_supply/bq27200-0/current_now", ok);
 
212
                return bq_read_value("/sys/class/power_supply/bq27200-0/current_now", ok) * 20 / 3570;
180
213
        else if ( reg == 0x0A ) // FLAGS
181
214
                return 0;
182
215
        else if ( reg == 0x16 ) // TTE
183
 
                return bq_read_value("/sys/class/power_supply/bq27200-0/time_to_empty_now", ok);
 
216
                return bq_read_value("/sys/class/power_supply/bq27200-0/time_to_empty_now", ok) / 60;
 
217
        else if ( reg == 0x26 ) // TTECP
 
218
                return bq_read_value("/sys/class/power_supply/bq27200-0/time_to_empty_avg", ok) / 60;
 
219
        else if ( reg == 0x18 ) // TTF
 
220
                return bq_read_value("/sys/class/power_supply/bq27200-0/time_to_full_now", ok) / 60;
 
221
        else if ( reg == 0x0C ) // NAC
 
222
                return 0;
 
223
        else if ( reg == 0x12 ) // LMD
 
224
                return 0;
 
225
        else if ( reg == 0x2A ) // CYCT
 
226
                return 0;
 
227
        else if ( reg == 0x22 ) // AE
 
228
                return 0;
 
229
        else if ( reg == 0x0B ) // RSOC
 
230
                return 0;
 
231
        else if ( reg == 0x76 ) // ILMD
 
232
                return 0;
184
233
 
185
234
        return 0;
186
235
 
187
236
}
188
237
 
189
 
int ioctl(int fd, int request, ...) {
 
238
int ioctl(int fd, int request, void * arg) {
190
239
 
191
 
        printf("ioclt: %d %d\n", fd, request);
 
240
#ifdef DEBUG
 
241
        fprintf(stderr, "libbqioctl: ioclt(%d, 0x%x, %p)\n", fd, request, arg);
 
242
#endif
192
243
 
193
244
        if ( fd >= 0 && fd <= (int)sizeof(slave_addr) && slave_addr[fd] >= 0 ) {
194
245
 
 
246
                printf("request=0x%x I2C_SLAVE=0x%x I2C_SMBUS=0x%x\n", request, I2C_SLAVE, I2C_SMBUS);
 
247
 
195
248
                if ( request == I2C_SLAVE ) {
196
249
 
197
 
                        slave_addr[fd] = *(&request + 1);
 
250
                        slave_addr[fd] = *((int *)(&arg));
 
251
 
 
252
#ifdef DEBUG
 
253
                        fprintf(stderr, "libbqioctl: change slave address for fd %d to 0x%x\n", fd, slave_addr[fd]);
 
254
#endif
198
255
 
199
256
                        if ( slave_addr[fd] == 0x55 )
200
257
                                return 0;
201
258
 
202
259
                } else if ( request == I2C_SMBUS && slave_addr[fd] == 0x55 ) {
203
260
 
204
 
                        struct i2c_smbus_ioctl_data * data = *((void **)(&request + 1));
 
261
                        struct i2c_smbus_ioctl_data * data = arg;
205
262
 
206
263
                        if ( data->read_write == I2C_SMBUS_READ ) {
207
264
 
 
265
#ifdef DEBUG
 
266
                                fprintf(stderr, "libbqioctl: read bq register %d\n", data->command);
 
267
#endif
 
268
 
208
269
                                int ok = 0;
209
270
                                int val = bq_read_reg(data->command, &ok);
210
271
 
216
277
                                else if ( data->size == I2C_SMBUS_WORD_DATA )
217
278
                                        data->data->word = 0xFFFF & val;
218
279
 
 
280
#ifdef DEBUG
 
281
                                fprintf(stderr, "libbqioctl: output value is %d\n", val);
 
282
#endif
 
283
 
219
284
                                return 0;
220
285
 
221
286
                        }
224
289
 
225
290
        }
226
291
 
227
 
        __builtin_return(__builtin_apply((void (*)())ioctl_libc, __builtin_apply_args(), sizeof(int) * 2 + sizeof(void *) * 3));
228
 
//      return ioctl_libc(fd, request, *(&request + 1));
 
292
        return ioctl_libc(fd, request, arg);
229
293
 
230
294
}