diff options
Diffstat (limited to 'bsps/sparc/shared/can/satcan.c')
-rw-r--r-- | bsps/sparc/shared/can/satcan.c | 714 |
1 files changed, 714 insertions, 0 deletions
diff --git a/bsps/sparc/shared/can/satcan.c b/bsps/sparc/shared/can/satcan.c new file mode 100644 index 0000000000..1655a36e36 --- /dev/null +++ b/bsps/sparc/shared/can/satcan.c @@ -0,0 +1,714 @@ +/* + * SatCAN FPGA driver + * + * COPYRIGHT (c) 2008. + * Cobham Gaisler AB. + * + * The license and distribution terms for this file may be + * found in the file LICENSE in this distribution or at + * http://www.rtems.org/license/LICENSE. + */ + +#include <rtems/libio.h> +#include <stdlib.h> +#include <stdio.h> +#include <string.h> +#include <bsp.h> +#include <rtems/bspIo.h> /* printk */ + +#include <bsp/satcan.h> +#include <ambapp.h> + +#ifndef GAISLER_SATCAN +#define GAISLER_SATCAN 0x080 +#endif + +#if !defined(SATCAN_DEVNAME) + #undef SATCAN_DEVNAME + #define SATCAN_DEVNAME "/dev/satcan" +#endif + +/* Enable debug output? */ +/* #define DEBUG */ + +#ifdef DEBUG +#define DBG(x...) printk(x) +#else +#define DBG(x...) +#endif + + +/* Defines related to DMA */ +#define ALIGN_2KMEM 32*1024 +#define ALIGN_8KMEM 128*1024 + +#define OFFSET_2K_LOW_POS 15 +#define OFFSET_8K_LOW_POS 17 + +#define DMA_2K_DATA_SELECT (1 << 14) +#define DMA_8K_DATA_SELECT (1 << 16) + +#define DMA_2K_DATA_OFFSET 16*1024 +#define DMA_8K_DATA_OFFSET 64*1024 + +/* Core register structures and defines */ + +/* Indexes to SatCAN registers in satcan array are declared in satcan.h*/ +/* Fields for some of the SatCAN FPGA registers */ + +/* CmdReg0 */ +#define CAN_TODn_Int_sel (1 << 5) + +/* CmdReg1 */ +#define Sel_2k_8kN (1 << 0) + +/* Read FIFO */ +#define FIFO_Full (1 << 8) +#define FIFO_Empty (1 << 9) + +/* DMA Ch_Enable */ +#define DMA_AutoInitDmaTx (1 << 3) +#define DMA_EnTx2 (1 << 2) +#define DMA_EnTx1 (1 << 1) +#define DMA_EnRx (1 << 0) + +/* SatCAN wrapper register fields */ +#define CTRL_BT_P 9 +#define CTRL_NODENO_P 5 +#define CTRL_DIS (1 << 2) +#define CTRL_DPS_P 1 +#define CTRL_RST (1 << 0) + +#define IRQ_AHB (1 << 8) +#define IRQ_PPS (1 << 7) +#define IRQ_M5 (1 << 6) +#define IRQ_M4 (1 << 5) +#define IRQ_M3 (1 << 4) +#define IRQ_M2 (1 << 3) +#define IRQ_M1 (1 << 2) +#define IRQ_SYNC (1 << 1) +#define IRQ_CAN (1 << 0) + +#define MSK_AHB (1 << 8) +#define MSK_PPS (1 << 7) +#define MSK_M5 (1 << 6) +#define MSK_M4 (1 << 5) +#define MSK_M3 (1 << 4) +#define MSK_M2 (1 << 3) +#define MSK_M1 (1 << 2) +#define MSK_SYNC (1 << 1) +#define MSK_CAN (1 << 0) + + + +struct satcan_regs { + volatile unsigned int satcan[32]; + volatile unsigned int ctrl; + volatile unsigned int irqpend; + volatile unsigned int irqmask; + volatile unsigned int membase; +}; + + +struct satcan_priv { + /* config */ + void *dmaptr; + unsigned char *alptr; + satcan_config *cfg; + + /* driver state */ + rtems_id devsem; + rtems_id txsem; + int open; + int txactive; + int dmaen; + int doff; + rtems_interval timeout; + int dmamode; +}; + +static struct satcan_regs *regs; +static struct satcan_priv *priv; + +static rtems_device_driver satcan_ioctl(rtems_device_major_number major, rtems_device_minor_number minor, void *arg); +static rtems_device_driver satcan_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg); +static rtems_device_driver satcan_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg); +static rtems_device_driver satcan_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg); +static rtems_device_driver satcan_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg); +static rtems_device_driver satcan_initialize(rtems_device_major_number major, rtems_device_minor_number unused, void *arg); + + +/* + * almalloc: allocate memory area of size sz aligned on sz boundary + * alptr: Utilized to return aligned pointer + * ptr: Unaligned pointer + * sz: Size of memory area + */ +static void almalloc(unsigned char **alptr, void **ptr, int sz) +{ + *ptr = calloc(1,2*sz); + *alptr = (unsigned char *) (((int)*ptr+sz) & ~(sz-1)); +} + +static rtems_isr satcan_interrupt_handler(rtems_vector_number v) +{ + unsigned int irq; + unsigned int fifo; + + irq = regs->irqpend; + + if (irq & IRQ_AHB && priv->cfg->ahb_irq_callback) { + priv->cfg->ahb_irq_callback(); + } + if (irq & IRQ_PPS && priv->cfg->pps_irq_callback) { + priv->cfg->pps_irq_callback(); + } + if (irq & IRQ_M5 && priv->cfg->m5_irq_callback) { + priv->cfg->m5_irq_callback(); + } + if (irq & IRQ_M4 && priv->cfg->m4_irq_callback) { + priv->cfg->m4_irq_callback(); + } + if (irq & IRQ_M3 && priv->cfg->m3_irq_callback) { + priv->cfg->m3_irq_callback(); + } + if (irq & IRQ_M2 && priv->cfg->m2_irq_callback) { + priv->cfg->m2_irq_callback(); + } + if (irq & IRQ_M1 && priv->cfg->m1_irq_callback) { + priv->cfg->m1_irq_callback(); + } + if (irq & IRQ_SYNC && priv->cfg->sync_irq_callback) { + priv->cfg->sync_irq_callback(); + } + if (irq & IRQ_CAN) { + fifo = regs->satcan[SATCAN_FIFO]; + if (!(fifo & FIFO_Empty) && priv->txactive && + (((fifo & 0xff) == SATCAN_IRQ_EOD1) || ((fifo & 0xff) == SATCAN_IRQ_EOD2))) { + rtems_semaphore_release(priv->txsem); + } + if (priv->cfg->can_irq_callback) + priv->cfg->can_irq_callback(fifo); + } +} + + + +static rtems_device_driver satcan_ioctl(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) +{ + rtems_libio_ioctl_args_t *ioarg = (rtems_libio_ioctl_args_t*)arg; + int *value; + rtems_interval *timeout; + satcan_regmod *regmod; + + DBG("SatCAN: IOCTL %d\n\r", ioarg->command); + + ioarg->ioctl_return = 0; + switch(ioarg->command) { + case SATCAN_IOC_DMA_2K: + DBG("SatCAN: ioctl: setting 2K DMA mode\n\r"); + free(priv->dmaptr); + almalloc(&priv->alptr, &priv->dmaptr, ALIGN_2KMEM); + if (priv->dmaptr == NULL) { + printk("SatCAN: Failed to allocate DMA memory\n\r"); + return RTEMS_NO_MEMORY; + } + + regs->membase = (unsigned int)priv->alptr; + regs->satcan[SATCAN_RAM_BASE] = (unsigned int)priv->alptr >> OFFSET_2K_LOW_POS; + regs->satcan[SATCAN_CMD1] = regs->satcan[SATCAN_CMD1] | Sel_2k_8kN; + break; + + case SATCAN_IOC_DMA_8K: + DBG("SatCAN: ioctl: setting 8K DMA mode\n\r"); + free(priv->dmaptr); + almalloc(&priv->alptr, &priv->dmaptr, ALIGN_8KMEM); + if (priv->dmaptr == NULL) { + printk("SatCAN: Failed to allocate DMA memory\n\r"); + return RTEMS_NO_MEMORY; + } + + regs->membase = (unsigned int)priv->alptr; + regs->satcan[SATCAN_RAM_BASE] = (unsigned int)priv->alptr >> OFFSET_8K_LOW_POS; + regs->satcan[SATCAN_CMD1] = regs->satcan[SATCAN_CMD1] & ~Sel_2k_8kN; + break; + + case SATCAN_IOC_GET_REG: + /* Get regmod structure from argument */ + regmod = (satcan_regmod*)ioarg->buffer; + DBG("SatCAN: ioctl: getting register %d\n\r", regmod->reg); + if (regmod->reg < 0) + return RTEMS_INVALID_NAME; + else if (regmod->reg <= SATCAN_FILTER_STOP) + regmod->val = regs->satcan[regmod->reg]; + else if (regmod->reg == SATCAN_WCTRL) + regmod->val = regs->ctrl; + else if (regmod->reg == SATCAN_WIPEND) + regmod->val = regs->irqpend; + else if (regmod->reg == SATCAN_WIMASK) + regmod->val = regs->irqmask; + else if (regmod->reg == SATCAN_WAHBADDR) + regmod->val = regs->membase; + else + return RTEMS_INVALID_NAME; + break; + + case SATCAN_IOC_SET_REG: + /* Get regmod structure from argument */ + regmod = (satcan_regmod*)ioarg->buffer; + DBG("SatCAN: ioctl: setting register %d, value %x\n\r", + regmod->reg, regmod->val); + if (regmod->reg < 0) + return RTEMS_INVALID_NAME; + else if (regmod->reg <= SATCAN_FILTER_STOP) + regs->satcan[regmod->reg] = regmod->val; + else if (regmod->reg == SATCAN_WCTRL) + regs->ctrl = regmod->val; + else if (regmod->reg == SATCAN_WIPEND) + regs->irqpend = regmod->val; + else if (regmod->reg == SATCAN_WIMASK) + regs->irqmask = regmod->val; + else if (regmod->reg == SATCAN_WAHBADDR) + regs->membase = regmod->val; + else + return RTEMS_INVALID_NAME; + break; + + case SATCAN_IOC_OR_REG: + /* Get regmod structure from argument */ + regmod = (satcan_regmod*)ioarg->buffer; + DBG("SatCAN: ioctl: or:ing register %d, with value %x\n\r", + regmod->reg, regmod->val); + if (regmod->reg < 0) + return RTEMS_INVALID_NAME; + else if (regmod->reg <= SATCAN_FILTER_STOP) + regs->satcan[regmod->reg] |= regmod->val; + else if (regmod->reg == SATCAN_WCTRL) + regs->ctrl |= regmod->val; + else if (regmod->reg == SATCAN_WIPEND) + regs->irqpend |= regmod->val; + else if (regmod->reg == SATCAN_WIMASK) + regs->irqmask |= regmod->val; + else if (regmod->reg == SATCAN_WAHBADDR) + regs->membase |= regmod->val; + else + return RTEMS_INVALID_NAME; + break; + + case SATCAN_IOC_AND_REG: + /* Get regmod structure from argument */ + regmod = (satcan_regmod*)ioarg->buffer; + DBG("SatCAN: ioctl: masking register %d, with value %x\n\r", + regmod->reg, regmod->val); + if (regmod->reg < 0) + return RTEMS_INVALID_NAME; + else if (regmod->reg <= SATCAN_FILTER_STOP) + regs->satcan[regmod->reg] &= regmod->val; + else if (regmod->reg == SATCAN_WCTRL) + regs->ctrl &= regmod->val; + else if (regmod->reg == SATCAN_WIPEND) + regs->irqpend &= regmod->val; + else if (regmod->reg == SATCAN_WIMASK) + regs->irqmask &= regmod->val; + else if (regmod->reg == SATCAN_WAHBADDR) + regs->membase &= regmod->val; + else + return RTEMS_INVALID_NAME; + break; + + case SATCAN_IOC_EN_TX1_DIS_TX2: + priv->dmaen = SATCAN_DMA_ENABLE_TX1; + break; + + case SATCAN_IOC_EN_TX2_DIS_TX1: + priv->dmaen = SATCAN_DMA_ENABLE_TX2; + break; + + case SATCAN_IOC_GET_DMA_MODE: + value = (int*)ioarg->buffer; + *value = priv->dmamode; + break; + + case SATCAN_IOC_SET_DMA_MODE: + value = (int*)ioarg->buffer; + if (*value != SATCAN_DMA_MODE_USER && *value != SATCAN_DMA_MODE_SYSTEM) { + DBG("SatCAN: ioctl: invalid DMA mode\n\r"); + return RTEMS_INVALID_NAME; + } + priv->dmamode = *value; + break; + + case SATCAN_IOC_ACTIVATE_DMA: + if (priv->dmamode != SATCAN_DMA_MODE_USER) { + DBG("SatCAN: ioctl: ACTIVATE_DMA: not in user mode\n\r"); + return RTEMS_INVALID_NAME; + } + value = (int*)ioarg->buffer; + if (*value != SATCAN_DMA_ENABLE_TX1 && *value != SATCAN_DMA_ENABLE_TX2) { + DBG("SatCAN: ioctl: ACTIVATE_DMA: Illegal channel\n\r"); + return RTEMS_INVALID_NAME; + } + regs->satcan[SATCAN_DMA] |= *value << 1; + break; + + case SATCAN_IOC_DEACTIVATE_DMA: + if (priv->dmamode != SATCAN_DMA_MODE_USER) { + DBG("SatCAN: ioctl: DEACTIVATE_DMA: not in user mode\n\r"); + return RTEMS_INVALID_NAME; + } + value = (int*)ioarg->buffer; + if (*value != SATCAN_DMA_ENABLE_TX1 && *value != SATCAN_DMA_ENABLE_TX2) { + DBG("SatCAN: ioctl: DEACTIVATE_DMA: Illegal channel\n\r"); + return RTEMS_INVALID_NAME; + } + regs->satcan[SATCAN_DMA] &= ~(*value << 1); + break; + + case SATCAN_IOC_GET_DOFFSET: + value = (int*)ioarg->buffer; + *value = priv->doff; + break; + + case SATCAN_IOC_SET_DOFFSET: + value = (int*)ioarg->buffer; + priv->doff = *value; + break; + + case SATCAN_IOC_GET_TIMEOUT: + timeout = (rtems_interval*)ioarg->buffer; + *timeout = priv->timeout; + break; + + case SATCAN_IOC_SET_TIMEOUT: + timeout = (rtems_interval*)ioarg->buffer; + priv->timeout = *timeout; + break; + + default: + return RTEMS_NOT_DEFINED; + } + + return RTEMS_SUCCESSFUL; +} + +static rtems_device_driver satcan_write(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) +{ + int i; + int doff; + int msgindex; + int messages; + rtems_libio_rw_args_t *rw_args=(rtems_libio_rw_args_t *) arg; + satcan_msg *msgs; + rtems_status_code status; + + DBG("SatCAN: Writing %d bytes from %p\n\r",rw_args->count,rw_args->buffer); + + if ((rw_args->count < sizeof(satcan_msg)) || (!rw_args->buffer)) { + DBG("SatCAN: write: returning EINVAL\n\r"); + return RTEMS_INVALID_NAME; /* EINVAL */ + } + + messages = rw_args->count / sizeof(satcan_msg); + msgs = (satcan_msg*)rw_args->buffer; + + /* Check that size matches any number of satcan_msg */ + if (rw_args->count % sizeof(satcan_msg)) { + DBG("SatCAN: write: count can not be evenly divided with satcan_msg size\n\r"); + return RTEMS_INVALID_NAME; /* EINVAL */ + } + + + /* DMA channel must be set if we are in system DMA mode */ + DBG("SatCAN: write: dma channel select is %x\n\r", priv->dmaen); + if (!priv->dmaen && priv->dmamode == SATCAN_DMA_MODE_SYSTEM) + return RTEMS_INVALID_NAME; /* EINVAL */ + + /* DMA must not be active */ + if (regs->satcan[SATCAN_DMA] & (DMA_EnTx1 | DMA_EnTx2 | DMA_AutoInitDmaTx)) { + DBG("SatCAN: write: DMA was active\n\r"); + rw_args->bytes_moved = 0; + return RTEMS_IO_ERROR; /* EIO */ + } + + doff = regs->satcan[SATCAN_CMD1] & Sel_2k_8kN ? DMA_2K_DATA_OFFSET : DMA_8K_DATA_OFFSET; + + for (msgindex = 0; msgindex < messages; msgindex++) { + /* Place header in DMA area */ + for (i = 0; i < SATCAN_HEADER_SIZE; i++) { + priv->alptr[priv->doff+8*msgindex+i] = msgs[msgindex].header[i]; + } + + /* Place data in DMA area */ + for (i = 0; i < SATCAN_PAYLOAD_SIZE; i++) + priv->alptr[priv->doff+doff+8*msgindex+i] = msgs[msgindex].payload[i]; + } + + if ((priv->dmaen & SATCAN_DMA_ENABLE_TX1) || priv->dmamode == SATCAN_DMA_MODE_USER) { + regs->satcan[SATCAN_DMA_TX_1_CUR] = 0; + regs->satcan[SATCAN_DMA_TX_1_END] = messages<<3; + } + + if ((priv->dmaen & SATCAN_DMA_ENABLE_TX2) || priv->dmamode == SATCAN_DMA_MODE_USER) { + regs->satcan[SATCAN_DMA_TX_2_CUR] = 0; + regs->satcan[SATCAN_DMA_TX_2_END] = messages<<3; + } + + /* If we are in DMA user mode we are done here, otherwise we block */ + if (priv->dmamode == SATCAN_DMA_MODE_SYSTEM) { + priv->txactive = 1; + + /* Enable DMA */ + regs->satcan[SATCAN_DMA] |= priv->dmaen << 1; + + /* Wait for TX interrupt */ + status = rtems_semaphore_obtain(priv->txsem, RTEMS_WAIT, priv->timeout); + + priv->txactive = 0; + + /* Disable activated Tx DMA */ + regs->satcan[SATCAN_DMA] &= ~(priv->dmaen << 1); + + if (status != RTEMS_SUCCESSFUL) { + rw_args->bytes_moved = 0; + return status; + } + } + + rw_args->bytes_moved = rw_args->count; + + return RTEMS_SUCCESSFUL; +} + +static rtems_device_driver satcan_read(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) +{ + char *buf; + int i; + int canid; + int messages; + rtems_libio_rw_args_t *rw_args = (rtems_libio_rw_args_t*)arg; + satcan_msg *ret; + + /* Check that there is room for the return */ + if (rw_args->count < sizeof(satcan_msg)) { + DBG("SatCAN: read: length of buffer must be at least %d, current is %d\n\r", + sizeof(satcan_msg) + sizeof(int), rw_args->count); + return RTEMS_INVALID_NAME; /* -EINVAL */ + } + + /* Check that size matches any number of satcan_msg */ + if (rw_args->count % sizeof(satcan_msg)) { + DBG("SatCAN: read: count can not be evenly divided with satcan_msg size\n\r"); + return RTEMS_INVALID_NAME; /* EINVAL */ + } + + messages = rw_args->count / sizeof(satcan_msg); + ret = (satcan_msg*)rw_args->buffer; + + DBG("SatCAN: read: reading %d messages to %p\n\r", messages, ret); + + for (i = 0; i < messages; i++) { + canid = (ret[i].header[1] << 8) | ret[i].header[0]; + + /* Copy message header from DMA header area to buffer */ + buf = (char*)((int)priv->alptr | (canid << 3)); + memcpy(ret[i].header, buf, SATCAN_HEADER_SIZE); + + DBG("SatCAN: read: copied header from %p to %p\n\r", buf, ret[i].header); + + /* Clear New Message Marker */ + buf[SATCAN_HEADER_NMM_POS] = 0; + + /* Copy message payload from DMA data area to buffer */ + buf = (char*)((int)buf | + (regs->satcan[SATCAN_CMD1] & Sel_2k_8kN ? DMA_2K_DATA_SELECT : DMA_8K_DATA_SELECT)); + memcpy(ret[i].payload, buf, SATCAN_PAYLOAD_SIZE); + + DBG("SatCAN: read: copied payload from %p to %p\n\r", buf, ret[i].payload); + } + rw_args->bytes_moved = rw_args->count; + + return RTEMS_SUCCESSFUL; +} + + +static rtems_device_driver satcan_close(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) +{ + DBG("SatCAN: Closing %d\n\r",minor); + + if (priv->open) { + regs->irqmask = 0; + regs->satcan[SATCAN_INT_EN] = 0; + regs->satcan[SATCAN_RX] = 0; + regs->satcan[SATCAN_DMA] = 0; + priv->open = 0; + priv->dmaen = 0; + priv->doff = 0; + priv->timeout = RTEMS_NO_TIMEOUT; + priv->dmamode = SATCAN_DMA_MODE_SYSTEM; + } + + return RTEMS_SUCCESSFUL; +} + + +static rtems_device_driver satcan_open(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) +{ + DBG("SatCAN: Opening %d\n\r",minor); + + rtems_semaphore_obtain(priv->devsem,RTEMS_WAIT, RTEMS_NO_TIMEOUT); + if (priv->open) { + rtems_semaphore_release(priv->devsem); + return RTEMS_RESOURCE_IN_USE; /* EBUSY */ + } + priv->open = 1; + rtems_semaphore_release(priv->devsem); + + /* Enable AHB and CAN IRQs in wrapper and EOD1, EOD2 and CAN critical IRQs in SatCAN core */ + regs->irqmask = MSK_AHB | MSK_CAN; + regs->satcan[SATCAN_INT_EN] = ((1 << SATCAN_IRQ_EOD1) | (1 << SATCAN_IRQ_EOD2) | + (1 << SATCAN_IRQ_CRITICAL)); + + /* Select can_int as IRQ source */ + regs->satcan[SATCAN_CMD0] = CAN_TODn_Int_sel; + /* CAN RX DMA Enable */ + regs->satcan[SATCAN_DMA] = 1; + /* CAN RX Enable */ + regs->satcan[SATCAN_RX] = 1; + + DBG("SatCAN: Opening %d success\n\r",minor); + + return RTEMS_SUCCESSFUL; +} + +static rtems_device_driver satcan_initialize(rtems_device_major_number major, rtems_device_minor_number minor, void *arg) +{ + struct ambapp_ahb_info d; + char fs_name[20]; + rtems_status_code status; + + DBG("SatCAN: Initialize..\n\r"); + + strcpy(fs_name, SATCAN_DEVNAME); + + /* Find core and initialize register pointer */ + if (!ambapp_find_ahbslv(&ambapp_plb, VENDOR_GAISLER, GAISLER_SATCAN, &d)) { + printk("SatCAN: Failed to find SatCAN core\n\r"); + return -1; + } + + status = rtems_io_register_name(fs_name, major, minor); + if (RTEMS_SUCCESSFUL != status) + rtems_fatal_error_occurred(status); + + regs = (struct satcan_regs*)d.start[0]; + + /* Set node number and DPS */ + regs->ctrl |= ((priv->cfg->nodeno & 0xf) << 5) | (priv->cfg->dps << 1); + + /* Reset core */ + regs->ctrl |= CTRL_RST; + + /* Allocate DMA area */ + almalloc(&priv->alptr, &priv->dmaptr, ALIGN_2KMEM); + if (priv->dmaptr == NULL) { + printk("SatCAN: Failed to allocate DMA memory\n\r"); + free(priv->cfg); + free(priv); + return -1; + } + + /* Wait until core reset has completed */ + while (regs->ctrl & CTRL_RST) + ; + + /* Initialize core registers, default is 2K messages */ + regs->membase = (unsigned int)priv->alptr; + regs->satcan[SATCAN_RAM_BASE] = (unsigned int)priv->alptr >> 15; + + DBG("regs->membase = %x\n\r", (unsigned int)priv->alptr); + DBG("regs->satcan[SATCAN_RAM_BASE] = %x\n\r", (unsigned int)priv->alptr >> 15); + + status = rtems_semaphore_create( + rtems_build_name('S', 'd', 'v', '0'), + 1, + RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \ + RTEMS_NO_PRIORITY_CEILING, + 0, + &priv->devsem); + if (status != RTEMS_SUCCESSFUL) { + printk("SatCAN: Failed to create dev semaphore (%d)\n\r", status); + free(priv->cfg); + free(priv); + return RTEMS_UNSATISFIED; + } + status = rtems_semaphore_create( + rtems_build_name('S', 't', 'x', '0'), + 0, + RTEMS_FIFO | RTEMS_SIMPLE_BINARY_SEMAPHORE | RTEMS_NO_INHERIT_PRIORITY | \ + RTEMS_NO_PRIORITY_CEILING, + 0, + &priv->txsem); + if (status != RTEMS_SUCCESSFUL) { + printk("SatCAN: Failed to create tx semaphore (%d)\n\r", status); + free(priv->cfg); + free(priv); + return RTEMS_UNSATISFIED; + } + + priv->txactive = 0; + priv->open = 0; + priv->dmaen = 0; + priv->doff = 0; + priv->timeout = RTEMS_NO_TIMEOUT; + priv->dmamode = SATCAN_DMA_MODE_SYSTEM; + + /* Register interrupt handler */ + set_vector(satcan_interrupt_handler, d.irq+0x10, 2); + + return RTEMS_SUCCESSFUL; +} + + + +#define SATCAN_DRIVER_TABLE_ENTRY { satcan_initialize, satcan_open, satcan_close, satcan_read, satcan_write, satcan_ioctl } + +static rtems_driver_address_table satcan_driver = SATCAN_DRIVER_TABLE_ENTRY; + +int satcan_register(satcan_config *conf) +{ + rtems_status_code r; + rtems_device_major_number m; + + DBG("SatCAN: satcan_register called\n\r"); + + /* Create private structure */ + if ((priv = malloc(sizeof(struct satcan_priv))) == NULL) { + printk("SatCAN driver could not allocate memory for priv structure\n\r"); + return -1; + } + + DBG("SatCAN: Creating local copy of config structure\n\r"); + if ((priv->cfg = malloc(sizeof(satcan_config))) == NULL) { + printk("SatCAN driver could not allocate memory for cfg structure\n\r"); + return 1; + } + memcpy(priv->cfg, conf, sizeof(satcan_config)); + + if ((r = rtems_io_register_driver(0, &satcan_driver, &m)) == RTEMS_SUCCESSFUL) { + DBG("SatCAN driver successfully registered, major: %d\n\r", m); + } else { + switch(r) { + case RTEMS_TOO_MANY: + printk("SatCAN rtems_io_register_driver failed: RTEMS_TOO_MANY\n\r"); break; + case RTEMS_INVALID_NUMBER: + printk("SatCAN rtems_io_register_driver failed: RTEMS_INVALID_NUMBER\n\r"); break; + case RTEMS_RESOURCE_IN_USE: + printk("SatCAN rtems_io_register_driver failed: RTEMS_RESOURCE_IN_USE\n\r"); break; + default: + printk("SatCAN rtems_io_register_driver failed\n\r"); + } + return 1; + } + + return 0; +} |