Initial support of IPECamera protocol

1 files changed, 324 insertions, 70 deletions

diff --git a/pci.c b/pci.c
index dfcdaf7..af3fc72 100644
--- a/pci.c
+++ b/pci.c
@@ -2,6 +2,7 @@
 
 #include <stdio.h>
 #include <string.h>
+#include <strings.h>
 #include <stdlib.h>
 #include <stdint.h>
 #include <stdarg.h>
@@ -10,6 +11,7 @@
 #include <sys/ioctl.h>
 #include <sys/mman.h>
 #include <errno.h>
+#include <assert.h>
 
 #include "driver/pciDriver.h"
 
@@ -18,135 +20,160 @@
 
 #include "pci.h"
 #include "ipecamera.h"
+#include "error.h"
 
-static pci_board_info board_info;
-static page_mask = -1;
+#define BIT_MASK(bits) ((1 << (bits)) - 1)
 
 
+//#define PCILIB_FILE_IO
+
+struct pcilib_s {
+    int handle;
+    
+    uintptr_t page_mask;
+    pci_board_info board_info;
+    pcilib_model_t model;
+
+    pcilib_bar_t reg_bar;
+    char *reg_space;
+    
+#ifdef PCILIB_FILE_IO
+    int file_io_handle;
+#endif /* PCILIB_FILE_IO */
+};
+
 static void pcilib_print_error(const char *msg, ...) {
     va_list va;
     
     va_start(va, msg);
     vprintf(msg, va);
     va_end(va);
+    printf("\n");
 }
 
-static void (*Error)(const char *msg, ...) = pcilib_print_error;
+void (*pcilib_error)(const char *msg, ...) = pcilib_print_error;
+void (*pcilib_warning)(const char *msg, ...) = pcilib_print_error;
 
-int pcilib_open(const char *device) {
-    int handle = open(device, O_RDWR);
-    return handle;
+int pcilib_set_error_handler(void (*err)(const char *msg, ...)) {
+    pcilib_error = err;
 }
 
-void pcilib_close(int handle) {
-    close(handle);
-}
+pcilib_t *pcilib_open(const char *device, pcilib_model_t model) {
+    pcilib_t *ctx = malloc(sizeof(pcilib_t));
 
-int pcilib_set_error_handler(void (*err)(const char *msg, ...)) {
-    Error = err;
+    if (ctx) {
+	ctx->handle = open(device, O_RDWR);
+	ctx->page_mask = (uintptr_t)-1;
+	ctx->model = model;
+	ctx->reg_space = NULL;
+    }
+
+    return ctx;
 }
 
-const pci_board_info *pcilib_get_board_info(int handle) {
+const pci_board_info *pcilib_get_board_info(pcilib_t *ctx) {
     int ret;
     
-    if (page_mask < 0) {
-	ret = ioctl( handle, PCIDRIVER_IOC_PCI_INFO, &board_info );
-	if (ret) Error("PCIDRIVER_IOC_PCI_INFO ioctl have failed");
+    if (ctx->page_mask ==  (uintptr_t)-1) {
+	ret = ioctl( ctx->handle, PCIDRIVER_IOC_PCI_INFO, &ctx->board_info );
+	if (ret) pcilib_error("PCIDRIVER_IOC_PCI_INFO ioctl have failed");
 	
-	page_mask = get_page_mask();
+	ctx->page_mask = pcilib_get_page_mask();
     }
     
-    return &board_info;
+    return &ctx->board_info;
 }
 
 
-pcilib_model_t pcilib_detect_model(int handle) {
+pcilib_model_t pcilib_get_model(pcilib_t *ctx) {
+    if (ctx->model == PCILIB_MODEL_DETECT) {
 	unsigned short vendor_id;
 	unsigned short device_id;
 
-    pcilib_get_board_info(handle);
+	const pci_board_info *board_info = pcilib_get_board_info(ctx);
 
-    if ((board_info.vendor_id == PCIE_XILINX_VENDOR_ID)&&(board_info.device_id ==  PCIE_IPECAMERA_DEVICE_ID)) return PCILIB_MODEL_IPECAMERA;
-    return PCILIB_MODEL_PCI;
+	if ((board_info->vendor_id == PCIE_XILINX_VENDOR_ID)&&(board_info->device_id == PCIE_IPECAMERA_DEVICE_ID))
+	    ctx->model = PCILIB_MODEL_IPECAMERA;
+	else
+	    ctx->model = PCILIB_MODEL_PCI;
+    }
+    
+    return ctx->model;
 }
 
-static int pcilib_detect_bar(int handle, unsigned long addr, int size) {
+static int pcilib_detect_bar(pcilib_t *ctx, uintptr_t addr, size_t size) {
     int ret,i;
 	
-    pcilib_get_board_info(handle);
+    const pci_board_info *board_info = pcilib_get_board_info(ctx);
 		
     for (i = 0; i < PCILIB_MAX_BANKS; i++) {
-	if ((addr >= board_info.bar_start[i])&&((board_info.bar_start[i] + board_info.bar_length[i]) >= (addr + size))) return i;
+	if ((addr >= board_info->bar_start[i])&&((board_info->bar_start[i] + board_info->bar_length[i]) >= (addr + size))) return i;
     }
 	
     return -1;
 }
 
-static void *pcilib_detect_address(int handle, int *bar, unsigned long *addr, int size) {
-    if (*bar < 0) {
-	*bar = pcilib_detect_bar(handle, *addr, size);
-	if (*bar < 0) Error("The requested data block at address 0x%x with size 0x%x does not belongs to any available memory bank", *addr, size);
+static void pcilib_detect_address(pcilib_t *ctx, pcilib_bar_t *bar, uintptr_t *addr, size_t size) {
+    const pci_board_info *board_info = pcilib_get_board_info(ctx);
+    
+    if (*bar == PCILIB_BAR_DETECT) {
+	*bar = pcilib_detect_bar(ctx, *addr, size);
+	if (*bar < 0) pcilib_error("The requested data block at address 0x%x with size 0x%x does not belongs to any available memory bank", *addr, size);
     } else {
-	pcilib_get_board_info(handle);
-	
-	if ((*addr < board_info.bar_start[*bar])||((board_info.bar_start[*bar] + board_info.bar_length[*bar]) < (((uintptr_t)*addr) + size))) {
-	    if ((board_info.bar_length[*bar]) >= (((uintptr_t)*addr) + size)) 
-		*addr += board_info.bar_start[*bar];
+	if ((*addr < board_info->bar_start[*bar])||((board_info->bar_start[*bar] + board_info->bar_length[*bar]) < (((uintptr_t)*addr) + size))) {
+	    if ((board_info->bar_length[*bar]) >= (((uintptr_t)*addr) + size)) 
+		*addr += board_info->bar_start[*bar];
 	    else
-		Error("The requested data block at address 0x%x with size 0x%x does not belong the specified memory bank (Bar %i: starting at 0x%x with size 0x%x)", *addr, size, *bar, board_info.bar_start[*bar], board_info.bar_length[*bar]);
+		pcilib_error("The requested data block at address 0x%x with size 0x%x does not belong the specified memory bank (Bar %i: starting at 0x%x with size 0x%x)", *addr, size, *bar, board_info->bar_start[*bar], board_info->bar_length[*bar]);
 	}
     }
     
-    *addr -= board_info.bar_start[*bar];
-    *addr += board_info.bar_start[*bar] & page_mask;
+    *addr -= board_info->bar_start[*bar];
+    *addr += board_info->bar_start[*bar] & ctx->page_mask;
 }
 
-
-#ifdef FILE_IO
-int file_io_handle;
-#endif /* FILE_IO */
-
-void *pcilib_map_bar(int handle, int bar) {
+void *pcilib_map_bar(pcilib_t *ctx, pcilib_bar_t bar) {
     void *res;
     int ret; 
 
-    pcilib_get_board_info(handle);
+    const pci_board_info *board_info = pcilib_get_board_info(ctx);
     
-    ret = ioctl( handle, PCIDRIVER_IOC_MMAP_MODE, PCIDRIVER_MMAP_PCI );
-    if (ret) Error("PCIDRIVER_IOC_MMAP_MODE ioctl have failed", bar);
+    ret = ioctl( ctx->handle, PCIDRIVER_IOC_MMAP_MODE, PCIDRIVER_MMAP_PCI );
+    if (ret) pcilib_error("PCIDRIVER_IOC_MMAP_MODE ioctl have failed", bar);
 
-    ret = ioctl( handle, PCIDRIVER_IOC_MMAP_AREA, PCIDRIVER_BAR0 + bar );
-    if (ret) Error("PCIDRIVER_IOC_MMAP_AREA ioctl have failed for bank %i", bar);
+    ret = ioctl( ctx->handle, PCIDRIVER_IOC_MMAP_AREA, PCIDRIVER_BAR0 + bar );
+    if (ret) pcilib_error("PCIDRIVER_IOC_MMAP_AREA ioctl have failed for bank %i", bar);
 
-#ifdef FILE_IO
-    file_io_handle = open("/root/drivers/pciDriver/data", O_RDWR);
-    res = mmap( 0, board_info.bar_length[bar], PROT_WRITE | PROT_READ, MAP_SHARED, file_io_handle, 0 );
+#ifdef PCILIB_FILE_IO
+    file_io_handle = open("/root/data", O_RDWR);
+    res = mmap( 0, board_info->bar_length[bar], PROT_WRITE | PROT_READ, MAP_SHARED, ctx->file_io_handle, 0 );
 #else
-    res = mmap( 0, board_info.bar_length[bar], PROT_WRITE | PROT_READ, MAP_SHARED, handle, 0 );
+    res = mmap( 0, board_info->bar_length[bar], PROT_WRITE | PROT_READ, MAP_SHARED, ctx->handle, 0 );
 #endif
-    if ((!res)||(res == MAP_FAILED)) Error("Failed to mmap data bank %i", bar);
+    if ((!res)||(res == MAP_FAILED)) pcilib_error("Failed to mmap data bank %i", bar);
 
     
     return res;
 }
 
-void pcilib_unmap_bar(int handle, int bar, void *data) {
-    munmap(data, board_info.bar_length[bar]);
-#ifdef FILE_IO
-    close(file_io_handle);
+void pcilib_unmap_bar(pcilib_t *ctx, pcilib_bar_t bar, void *data) {
+    const pci_board_info *board_info = pcilib_get_board_info(ctx);
+
+    munmap(data, board_info->bar_length[bar]);
+#ifdef PCILIB_FILE_IO
+    close(ctx->file_io_handle);
 #endif
 }
 
-int pcilib_read(void *buf, int handle, int bar, unsigned long addr, int size) {
+int pcilib_read(pcilib_t *ctx, pcilib_bar_t bar, uintptr_t addr, size_t size, void *buf) {
     int i;
     void *data;
     unsigned int offset;
     char local_buf[size];
     
 
-
-    pcilib_detect_address(handle, &bar, &addr, size);
-    data = pcilib_map_bar(handle, bar);
+    pcilib_detect_address(ctx, &bar, &addr, size);
+    data = pcilib_map_bar(ctx, bar);
 
 /*
     for (i = 0; i < size/4; i++)  {
@@ -156,35 +183,262 @@ int pcilib_read(void *buf, int handle, int bar, unsigned long addr, int size) {
 
     pcilib_memcpy(buf, data + addr, size);
     
-    pcilib_unmap_bar(handle, bar, data);    
+    pcilib_unmap_bar(ctx, bar, data);    
 }
 
-int pcilib_write(void *buf, int handle, int bar, unsigned long addr, int size) {
+int pcilib_write(pcilib_t *ctx, pcilib_bar_t bar, uintptr_t addr, size_t size, void *buf) {
     int i;
     void *data;
     unsigned int offset;
     char local_buf[size];
     
 
-    pcilib_detect_address(handle, &bar, &addr, size);
-    data = pcilib_map_bar(handle, bar);
+    pcilib_detect_address(ctx, &bar, &addr, size);
+    data = pcilib_map_bar(ctx, bar);
 
     pcilib_memcpy(data + addr, buf, size);
     
-    pcilib_unmap_bar(handle, bar, data);    
+    pcilib_unmap_bar(ctx, bar, data);    
 }
 
 
-int pcilib_find_register(pcilib_model_t model, const char *reg) {
-    int i;
-
-    pcilib_register_t *registers =  pcilib_model_description[model].registers;
+    // FIXME create hash during map_register space
+pcilib_register_t pcilib_find_register(pcilib_t *ctx, const char *reg) {
+    pcilib_register_t i;
     
-    for (i = 0; registers[i].size; i++) {
+    pcilib_model_t model = pcilib_get_model(ctx);
+    
+    pcilib_register_description_t *registers =  pcilib_model[model].registers;
+    
+    for (i = 0; registers[i].bits; i++) {
 	if (!strcasecmp(registers[i].name, reg)) return i;
     }
     
     return -1;
 };
 
+static pcilib_register_bank_t pcilib_find_bank(pcilib_t *ctx, pcilib_register_bank_addr_t bank) {
+    pcilib_register_bank_t i;
+    pcilib_register_bank_description_t *banks = pcilib_model[ctx->model].banks;
+
+    for (i = 0; banks[i].access; i++)
+	if (banks[i].addr == bank) return i;
+	
+    return -1;
+}
+
+
+static int pcilib_map_register_space(pcilib_t *ctx) {
+    if (!ctx->reg_space)  {
+	pcilib_register_bank_description_t *banks = pcilib_model[ctx->model].banks;
+
+	if ((banks)&&(banks[0].access)) {
+	    void *reg_space;
+	    
+	    uintptr_t addr = banks[0].read_addr;
+	    pcilib_bar_t bar = PCILIB_BAR_DETECT;
+	    
+	    pcilib_detect_address(ctx, &bar, &addr, 1);
+	    reg_space = pcilib_map_bar(ctx, bar);
+
+	    uint32_t buf[2];
+	    pcilib_memcpy(&buf, reg_space, 8);
+	    
+	    if (reg_space) {
+	        ctx->reg_bar = bar;
+		ctx->reg_space = reg_space;
+
+		return 0;
+	    }
+	}
+	
+	return -1;
+    }
+    
+    return 0;
+}
+	    
+	
+static void pcilib_unmap_register_space(pcilib_t *ctx) {
+    if (ctx->reg_space) {
+	pcilib_unmap_bar(ctx, ctx->reg_bar, ctx->reg_space);
+	ctx->reg_space = NULL;
+    }
+}
+
+char  *pcilib_resolve_register_address(pcilib_t *ctx, uintptr_t addr) {
+    size_t offset = addr - ctx->board_info.bar_start[ctx->reg_bar];
+    if (offset < ctx->board_info.bar_length[ctx->reg_bar]) {
+	return ctx->reg_space + offset + (ctx->board_info.bar_start[ctx->reg_bar] & ctx->page_mask);
+    }
+    return NULL;
+}
+
+
+void pcilib_close(pcilib_t *ctx) {
+    if (ctx) {
+	if (ctx->reg_space) pcilib_unmap_register_space(ctx);
+	close(ctx->handle);
+	free(ctx);
+    }
+}
+
+
+
+
+static int pcilib_read_register_space_internal(pcilib_t *ctx, pcilib_register_bank_t bank, pcilib_register_addr_t addr, size_t n, uint8_t bits, pcilib_register_value_t *buf) {
+    int err;
+    int rest;
+    size_t i;
+    
+    pcilib_register_bank_description_t *b = pcilib_model[ctx->model].banks + bank;
+
+    assert(bits < 8 * sizeof(pcilib_register_value_t));
+
+    err = pcilib_map_register_space(ctx);
+    if (err) {
+	pcilib_error("Failed to map the register space");
+	return err;
+    }
+    
+    //n += bits / b->access;
+    //bits %= b->access; 
+    
+    for (i = 0; i < n; i++) {
+	err = pcilib_protocol[b->protocol].read(ctx, b, addr + i, b->access, buf + i);
+	if (err) break;
+    }
+    
+    if ((bits > 0)&&(!err)) err = pcilib_protocol[b->protocol].read(ctx, b, addr + n, bits, buf + n);
+    
+    return err;
+}
+
+int pcilib_read_register_space(pcilib_t *ctx, pcilib_register_bank_addr_t bank_addr, pcilib_register_addr_t addr, size_t n, pcilib_register_value_t *buf) {
+    pcilib_register_bank_t bank = pcilib_find_bank(ctx, bank_addr);
+    return pcilib_read_register_space_internal(ctx, bank, addr, n, 0, buf);
+}
+
+int pcilib_read_register_by_id(pcilib_t *ctx, pcilib_register_t reg, pcilib_register_value_t *value) {
+    int err;
+    size_t i, n, bits;
+    pcilib_register_value_t res;
+    pcilib_register_description_t *r;
+    pcilib_register_bank_description_t *b;
+
+    r = pcilib_model[ctx->model].registers + reg;
+    b = pcilib_model[ctx->model].banks + r->bank;
+    
+    n = r->bits / b->access;
+    bits = r->bits % b->access; 
+
+    pcilib_register_value_t buf[n + 1];
+    err = pcilib_read_register_space_internal(ctx, r->bank, r->addr, n, bits, buf);
+
+    if (b->endianess) {
+	pcilib_error("Big-endian byte order support is not implemented");
+	return PCILIB_ERROR_NOTSUPPORTED;
+    } else {
+	res = 0;
+	if (bits) ++n;
+	for (i = 0; i < n; i++) {
+	    res |= buf[i] << (i * b->access);
+	}
+    }
+    
+    *value = res;
+    
+    return err;
+}
+
+
+int pcilib_read_register(pcilib_t *ctx, const char *regname, pcilib_register_value_t *value) {
+    int err;
+    int reg;
+    
+    reg = pcilib_find_register(ctx, regname);
+    if (reg < 0) {
+	pcilib_error("Register (%s) is not found", regname);
+	return PCILIB_ERROR_NOTFOUND;
+    }
+    
+    return pcilib_read_register_by_id(ctx, reg, value);
+
+//    registers[reg].bank
+//    printf("%li %li", sizeof(pcilib_model[ctx->model].banks), sizeof(pcilib_register_bank_description_t));
+}
+
+
+static int pcilib_write_register_space_internal(pcilib_t *ctx, pcilib_register_bank_t bank, pcilib_register_addr_t addr, size_t n, uint8_t bits, pcilib_register_value_t *buf) {
+    int err;
+    int rest;
+    size_t i;
+    
+    pcilib_register_bank_description_t *b = pcilib_model[ctx->model].banks + bank;
+
+    assert(bits < 8 * sizeof(pcilib_register_value_t));
+
+    err = pcilib_map_register_space(ctx);
+    if (err) {
+	pcilib_error("Failed to map the register space");
+	return err;
+    }
+    
+    //n += bits / b->access;
+    //bits %= b->access; 
+    
+    for (i = 0; i < n; i++) {
+	err = pcilib_protocol[b->protocol].write(ctx, b, addr + i, b->access, buf[i]);
+	if (err) break;
+    }
+    
+    if ((bits > 0)&&(!err)) err = pcilib_protocol[b->protocol].write(ctx, b, addr + n, bits, buf[n]);
+    
+    return err;
+}
+
+int pcilib_write_register_by_id(pcilib_t *ctx, pcilib_register_t reg, pcilib_register_value_t value) {
+    int err;
+    size_t i, n, bits;
+    pcilib_register_value_t res;
+    pcilib_register_description_t *r;
+    pcilib_register_bank_description_t *b;
+
+    r = pcilib_model[ctx->model].registers + reg;
+    b = pcilib_model[ctx->model].banks + r->bank;
+    
+    n = r->bits / b->access;
+    bits = r->bits % b->access; 
+
+    pcilib_register_value_t buf[n + 1];
+    memset(buf, 0, (n + 1) * sizeof(pcilib_register_value_t));
+    
+    if (b->endianess) {
+	pcilib_error("Big-endian byte order support is not implemented");
+	return PCILIB_ERROR_NOTSUPPORTED;
+    } else {
+	for (i = 0, res = value; (res > 0)&&(i <= n); ++i) {
+	    buf[i] = res & BIT_MASK(b->access);
+	    res >>= b->access;
+	}
+	
+	if (res) {
+	    pcilib_error("Value %i is to big to fit in the register %s", value, r->name);
+	    return PCILIB_ERROR_OUTOFRANGE;
+	}
+    }
+    
+    err = pcilib_write_register_space_internal(ctx, r->bank, r->addr, n, bits, buf);
+    return err;
+}
+
+int pcilib_write_register(pcilib_t *ctx, const char *regname, pcilib_register_value_t value) {
+    int err;
+    int reg;
+    
+    reg = pcilib_find_register(ctx, regname);
+    if (reg < 0) pcilib_error("Register (%s) is not found", regname);
+
+    return pcilib_write_register_by_id(ctx, reg, value);
+}