/alps/ipecamera

To get this branch, use:
bzr branch http://darksoft.org/webbzr/alps/ipecamera

« back to all changes in this revision

Viewing changes to register.c

  • Committer: Suren A. Chilingaryan
  • Date: 2015-04-27 00:28:57 UTC
  • Revision ID: csa@suren.me-20150427002857-82fk6r3e8rfgy4wr
First stand-alone ipecamera implementation

Show diffs side-by-side

added added

removed removed

Lines of Context:
register.c
1
 
#include <stdio.h>
2
 
#include <string.h>
3
 
#include <strings.h>
4
 
#include <stdlib.h>
5
 
#include <stdint.h>
6
 
#include <stdarg.h>
7
 
#include <fcntl.h>
8
 
#include <unistd.h>
9
 
#include <sys/ioctl.h>
10
 
#include <sys/mman.h>
11
 
#include <arpa/inet.h>
12
 
#include <errno.h>
13
 
#include <assert.h>
14
 
 
15
 
#include "pci.h"
16
 
 
17
 
#include "tools.h"
18
 
#include "error.h"
19
 
 
20
 
int pcilib_add_registers(pcilib_t *ctx, size_t n, pcilib_register_description_t *registers) {
21
 
    pcilib_register_description_t *regs;
22
 
    size_t size, n_present = 0;
23
 
 
24
 
    if (!n) {
25
 
        for (n = 0; registers[n].bits; n++);
26
 
    }
27
 
 
28
 
    if (ctx->model_info.registers == pcilib_model[ctx->model].registers) {
29
 
        for (n_present = 0; ctx->model_info.registers[n_present].bits; n_present++);
30
 
        for (size = 1024; size < 2 * (n + n_present + 1); size<<=1);
31
 
        regs = (pcilib_register_description_t*)malloc(size * sizeof(pcilib_register_description_t));
32
 
        if (!regs) return PCILIB_ERROR_MEMORY;
33
 
        
34
 
        ctx->model_info.registers = regs;
35
 
        ctx->num_reg = n + n_present;
36
 
        ctx->alloc_reg = size;
37
 
        
38
 
        memcpy(ctx->model_info.registers, pcilib_model[ctx->model].registers, (n_present + 1) * sizeof(pcilib_register_description_t));
39
 
    } else {
40
 
        n_present = ctx->num_reg;
41
 
        if ((n_present + n + 1) > ctx->alloc_reg) {
42
 
            for (size = ctx->alloc_reg; size < 2 * (n + n_present + 1); size<<=1);
43
 
 
44
 
            regs = (pcilib_register_description_t*)realloc(ctx->model_info.registers, size * sizeof(pcilib_register_description_t));
45
 
            if (!regs) return PCILIB_ERROR_MEMORY;
46
 
 
47
 
            ctx->model_info.registers = regs;
48
 
            ctx->alloc_reg = size;
49
 
        }
50
 
        ctx->num_reg += n;
51
 
    }
52
 
 
53
 
    memcpy(ctx->model_info.registers + ctx->num_reg, ctx->model_info.registers + n_present, sizeof(pcilib_register_description_t));
54
 
    memcpy(ctx->model_info.registers + n_present, registers, n * sizeof(pcilib_register_description_t));
55
 
 
56
 
    return 0;
57
 
}
58
 
 
59
 
pcilib_register_bank_t pcilib_find_bank_by_addr(pcilib_t *ctx, pcilib_register_bank_addr_t bank) {
60
 
    pcilib_register_bank_t i;
61
 
    pcilib_model_description_t *model_info = pcilib_get_model_description(ctx);
62
 
    pcilib_register_bank_description_t *banks = model_info->banks;
63
 
 
64
 
    for (i = 0; banks[i].access; i++)
65
 
        if (banks[i].addr == bank) return i;
66
 
 
67
 
    return -1;
68
 
}
69
 
 
70
 
pcilib_register_bank_t pcilib_find_bank_by_name(pcilib_t *ctx, const char *bankname) {
71
 
    pcilib_register_bank_t i;
72
 
    pcilib_model_description_t *model_info = pcilib_get_model_description(ctx);
73
 
    pcilib_register_bank_description_t *banks = model_info->banks;
74
 
 
75
 
    for (i = 0; banks[i].access; i++)
76
 
        if (!strcasecmp(banks[i].name, bankname)) return i;
77
 
 
78
 
    return -1;
79
 
}
80
 
 
81
 
pcilib_register_bank_t pcilib_find_bank(pcilib_t *ctx, const char *bank) {
82
 
    pcilib_register_bank_t res;
83
 
    unsigned long addr;
84
 
    
85
 
    if (!bank) {
86
 
        pcilib_model_description_t *model_info = pcilib_get_model_description(ctx);
87
 
        pcilib_register_bank_description_t *banks = model_info->banks;
88
 
        if ((banks)&&(banks[0].access)) return (pcilib_register_bank_t)0;
89
 
        return -1;
90
 
    }
91
 
    
92
 
    if (pcilib_isxnumber(bank)&&(sscanf(bank,"%lx", &addr) == 1)) {
93
 
        res = pcilib_find_bank_by_addr(ctx, addr);
94
 
        if (res != PCILIB_REGISTER_BANK_INVALID) return res;
95
 
    }
96
 
    
97
 
    return pcilib_find_bank_by_name(ctx, bank);
98
 
}
99
 
 
100
 
    // FIXME create hash during map_register space
101
 
pcilib_register_t pcilib_find_register(pcilib_t *ctx, const char *bank, const char *reg) {
102
 
    pcilib_register_t i;
103
 
    pcilib_register_bank_t bank_id;
104
 
    pcilib_register_bank_addr_t bank_addr = 0;
105
 
 
106
 
    pcilib_model_description_t *model_info = pcilib_get_model_description(ctx);
107
 
    pcilib_register_description_t *registers =  model_info->registers;
108
 
    
109
 
    if (bank) {
110
 
        bank_id = pcilib_find_bank(ctx, bank);
111
 
        if (bank_id == PCILIB_REGISTER_BANK_INVALID) {
112
 
            pcilib_error("Invalid bank (%s) is specified", bank);
113
 
            return -1;
114
 
        }
115
 
        
116
 
        bank_addr = model_info->banks[bank_id].addr;
117
 
    }
118
 
    
119
 
    for (i = 0; registers[i].bits; i++) {
120
 
        if ((!strcasecmp(registers[i].name, reg))&&((!bank)||(registers[i].bank == bank_addr))) return i;
121
 
    }
122
 
    
123
 
    if ((ctx->model_info.dma_api)&&(!ctx->dma_ctx)&&(pcilib_get_dma_info(ctx))) {
124
 
        registers =  model_info->registers;
125
 
 
126
 
        for (; registers[i].bits; i++) {
127
 
            if ((!strcasecmp(registers[i].name, reg))&&((!bank)||(registers[i].bank == bank_addr))) return i;
128
 
        }
129
 
    }
130
 
    
131
 
    return (pcilib_register_t)-1;
132
 
};
133
 
 
134
 
static int pcilib_read_register_space_internal(pcilib_t *ctx, pcilib_register_bank_t bank, pcilib_register_addr_t addr, size_t n, pcilib_register_size_t offset, pcilib_register_size_t bits, pcilib_register_value_t *buf) {
135
 
    int err;
136
 
    size_t i;
137
 
    
138
 
    pcilib_model_description_t *model_info = pcilib_get_model_description(ctx);
139
 
    pcilib_register_bank_description_t *b = model_info->banks + bank;
140
 
    
141
 
    int access = b->access / 8;
142
 
 
143
 
    assert(bits < 8 * sizeof(pcilib_register_value_t));
144
 
    
145
 
    if (((addr + n) > b->size)||(((addr + n) == b->size)&&(bits))) {
146
 
        if ((b->format)&&(strchr(b->format, 'x')))
147
 
            pcilib_error("Accessing register (%u regs at addr 0x%x) out of register space (%u registers total)", bits?(n+1):n, addr, b->size);
148
 
        else 
149
 
            pcilib_error("Accessing register (%u regs at addr %u) out of register space (%u registers total)", bits?(n+1):n, addr, b->size);
150
 
        return PCILIB_ERROR_OUTOFRANGE;
151
 
    }
152
 
 
153
 
    err = pcilib_map_register_space(ctx);
154
 
    if (err) {
155
 
        pcilib_error("Failed to map the register space");
156
 
        return err;
157
 
    }
158
 
    
159
 
    //n += bits / b->access;
160
 
    //bits %= b->access; 
161
 
    
162
 
    for (i = 0; i < n; i++) {
163
 
        err = pcilib_protocol[b->protocol].read(ctx, b, addr + i * access, buf + i);
164
 
        if (err) break;
165
 
    }
166
 
    
167
 
    if ((bits > 0)&&(!err)) {
168
 
        pcilib_register_value_t val = 0;
169
 
        err = pcilib_protocol[b->protocol].read(ctx, b, addr + n * access, &val);
170
 
 
171
 
        val = (val >> offset)&BIT_MASK(bits);
172
 
        memcpy(buf + n, &val, sizeof(pcilib_register_value_t));
173
 
    }
174
 
    
175
 
    return err;
176
 
}
177
 
 
178
 
int pcilib_read_register_space(pcilib_t *ctx, const char *bank, pcilib_register_addr_t addr, size_t n, pcilib_register_value_t *buf) {
179
 
    pcilib_register_bank_t bank_id = pcilib_find_bank(ctx, bank);
180
 
    if (bank_id == PCILIB_REGISTER_BANK_INVALID) {
181
 
        if (bank) pcilib_error("Invalid register bank is specified (%s)", bank);
182
 
        else pcilib_error("Register bank should be specified");
183
 
        return PCILIB_ERROR_INVALID_BANK;
184
 
    }
185
 
    
186
 
    return pcilib_read_register_space_internal(ctx, bank_id, addr, n, 0, 0, buf);
187
 
}
188
 
 
189
 
int pcilib_read_register_by_id(pcilib_t *ctx, pcilib_register_t reg, pcilib_register_value_t *value) {
190
 
    int err;
191
 
    size_t i, n;
192
 
    pcilib_register_size_t bits;
193
 
    pcilib_register_value_t res;
194
 
    pcilib_register_bank_t bank;
195
 
    pcilib_register_description_t *r;
196
 
    pcilib_register_bank_description_t *b;
197
 
    pcilib_model_description_t *model_info = pcilib_get_model_description(ctx);
198
 
 
199
 
    r = model_info->registers + reg;
200
 
    
201
 
    bank = pcilib_find_bank_by_addr(ctx, r->bank);
202
 
    if (bank == PCILIB_REGISTER_BANK_INVALID) return PCILIB_ERROR_INVALID_BANK;
203
 
    
204
 
    b = model_info->banks + bank;
205
 
    
206
 
    n = r->bits / b->access;
207
 
    bits = r->bits % b->access; 
208
 
 
209
 
    pcilib_register_value_t buf[n + 1];
210
 
    err = pcilib_read_register_space_internal(ctx, bank, r->addr, n, r->offset, bits, buf);
211
 
 
212
 
    if ((b->endianess == PCILIB_BIG_ENDIAN)||((b->endianess == PCILIB_HOST_ENDIAN)&&(ntohs(1) == 1))) {
213
 
        pcilib_error("Big-endian byte order support is not implemented");
214
 
        return PCILIB_ERROR_NOTSUPPORTED;
215
 
    } else {
216
 
        res = 0;
217
 
        if (bits) ++n;
218
 
        for (i = 0; i < n; i++) {
219
 
            res |= buf[i] << (i * b->access);
220
 
        }
221
 
    }
222
 
    
223
 
    *value = res;
224
 
    
225
 
    return err;
226
 
}
227
 
 
228
 
 
229
 
int pcilib_read_register(pcilib_t *ctx, const char *bank, const char *regname, pcilib_register_value_t *value) {
230
 
    int reg;
231
 
    
232
 
    reg = pcilib_find_register(ctx, bank, regname);
233
 
    if (reg < 0) {
234
 
        pcilib_error("Register (%s) is not found", regname);
235
 
        return PCILIB_ERROR_NOTFOUND;
236
 
    }
237
 
    
238
 
    return pcilib_read_register_by_id(ctx, reg, value);
239
 
}
240
 
 
241
 
 
242
 
static int pcilib_write_register_space_internal(pcilib_t *ctx, pcilib_register_bank_t bank, pcilib_register_addr_t addr, size_t n, pcilib_register_size_t offset, pcilib_register_size_t bits, pcilib_register_value_t rwmask, pcilib_register_value_t *buf) {
243
 
    int err;
244
 
    size_t i;
245
 
    
246
 
    pcilib_model_description_t *model_info = pcilib_get_model_description(ctx);
247
 
    pcilib_register_bank_description_t *b = model_info->banks + bank;
248
 
 
249
 
    int access = b->access / 8;
250
 
 
251
 
    assert(bits < 8 * sizeof(pcilib_register_value_t));
252
 
 
253
 
    if (((addr + n) > b->size)||(((addr + n) == b->size)&&(bits))) {
254
 
        if ((b->format)&&(strchr(b->format, 'x')))
255
 
            pcilib_error("Accessing register (%u regs at addr 0x%x) out of register space (%u registers total)", bits?(n+1):n, addr, b->size);
256
 
        else 
257
 
            pcilib_error("Accessing register (%u regs at addr %u) out of register space (%u registers total)", bits?(n+1):n, addr, b->size);
258
 
        return PCILIB_ERROR_OUTOFRANGE;
259
 
    }
260
 
 
261
 
    err = pcilib_map_register_space(ctx);
262
 
    if (err) {
263
 
        pcilib_error("Failed to map the register space");
264
 
        return err;
265
 
    }
266
 
    
267
 
    //n += bits / b->access;
268
 
    //bits %= b->access; 
269
 
    
270
 
    for (i = 0; i < n; i++) {
271
 
        err = pcilib_protocol[b->protocol].write(ctx, b, addr + i * access, buf[i]);
272
 
        if (err) break;
273
 
    }
274
 
    
275
 
    if ((bits > 0)&&(!err)) {
276
 
        pcilib_register_value_t val = (buf[n]&BIT_MASK(bits))<<offset;
277
 
        pcilib_register_value_t mask = BIT_MASK(bits)<<offset;
278
 
 
279
 
        if (~mask&rwmask) {
280
 
            pcilib_register_value_t rval;
281
 
            
282
 
            err = pcilib_protocol[b->protocol].read(ctx, b, addr + n * access, &rval); 
283
 
            if (err) return err;
284
 
            
285
 
            val |= (rval & rwmask & ~mask);
286
 
        }
287
 
        
288
 
        err = pcilib_protocol[b->protocol].write(ctx, b, addr + n * access, val);
289
 
    }
290
 
    
291
 
    return err;
292
 
}
293
 
 
294
 
int pcilib_write_register_space(pcilib_t *ctx, const char *bank, pcilib_register_addr_t addr, size_t n, pcilib_register_value_t *buf) {
295
 
    pcilib_register_bank_t bank_id = pcilib_find_bank(ctx, bank);
296
 
    if (bank_id == PCILIB_REGISTER_BANK_INVALID) {
297
 
        if (bank) pcilib_error("Invalid register bank is specified (%s)", bank);
298
 
        else pcilib_error("Register bank should be specified");
299
 
        return PCILIB_ERROR_INVALID_BANK;
300
 
    }
301
 
    
302
 
    return pcilib_write_register_space_internal(ctx, bank_id, addr, n, 0, 0, 0, buf);
303
 
}
304
 
 
305
 
 
306
 
int pcilib_write_register_by_id(pcilib_t *ctx, pcilib_register_t reg, pcilib_register_value_t value) {
307
 
    int err;
308
 
    size_t i, n;
309
 
    pcilib_register_size_t bits;
310
 
    pcilib_register_bank_t bank;
311
 
    pcilib_register_value_t res;
312
 
    pcilib_register_description_t *r;
313
 
    pcilib_register_bank_description_t *b;
314
 
    pcilib_model_description_t *model_info = pcilib_get_model_description(ctx);
315
 
 
316
 
    r = model_info->registers + reg;
317
 
 
318
 
    bank = pcilib_find_bank_by_addr(ctx, r->bank);
319
 
    if (bank == PCILIB_REGISTER_BANK_INVALID) return PCILIB_ERROR_INVALID_BANK;
320
 
 
321
 
    b = model_info->banks + bank;
322
 
    
323
 
    n = r->bits / b->access;
324
 
    bits = r->bits % b->access; 
325
 
 
326
 
    pcilib_register_value_t buf[n + 1];
327
 
    memset(buf, 0, (n + 1) * sizeof(pcilib_register_value_t));
328
 
    
329
 
    if ((b->endianess == PCILIB_BIG_ENDIAN)||((b->endianess == PCILIB_HOST_ENDIAN)&&(ntohs(1) == 1))) {
330
 
        pcilib_error("Big-endian byte order support is not implemented");
331
 
        return PCILIB_ERROR_NOTSUPPORTED;
332
 
    } else {
333
 
        if (b->access == sizeof(pcilib_register_value_t) * 8) {
334
 
            buf[0] = value;
335
 
        } else {
336
 
            for (i = 0, res = value; (res > 0)&&(i <= n); ++i) {
337
 
                buf[i] = res & BIT_MASK(b->access);
338
 
                res >>= b->access;
339
 
            }
340
 
        
341
 
            if (res) {
342
 
                pcilib_error("Value %i is too big to fit in the register %s", value, r->name);
343
 
                return PCILIB_ERROR_OUTOFRANGE;
344
 
            }
345
 
        }
346
 
    }
347
 
 
348
 
    err = pcilib_write_register_space_internal(ctx, bank, r->addr, n, r->offset, bits, r->rwmask, buf);
349
 
    return err;
350
 
}
351
 
 
352
 
int pcilib_write_register(pcilib_t *ctx, const char *bank, const char *regname, pcilib_register_value_t value) {
353
 
    int reg;
354
 
    
355
 
    reg = pcilib_find_register(ctx, bank, regname);
356
 
    if (reg < 0) {
357
 
        pcilib_error("Register (%s) is not found", regname);
358
 
        return PCILIB_ERROR_NOTFOUND;
359
 
    }
360
 
 
361
 
    return pcilib_write_register_by_id(ctx, reg, value);
362
 
}