From 45b3ad9101e4e75af54e2d3bc25be419d3df7224 Mon Sep 17 00:00:00 2001 From: Jiri Vanek Date: Mon, 6 Feb 2012 16:56:05 +0100 Subject: [PATCH] Direct approach to CAN registers was replaced by calling functions can_write_register and can_read_register, which work with register base in struct canchip_t. --- embedded/app/usbcan/can/lpc17xx_can.h | 82 ++++++----- embedded/app/usbcan/lpc17xx_can.c | 190 +++++++++++++------------- embedded/app/usbcan/main.c | 24 +--- 3 files changed, 140 insertions(+), 156 deletions(-) diff --git a/embedded/app/usbcan/can/lpc17xx_can.h b/embedded/app/usbcan/can/lpc17xx_can.h index d4683e5..bd2d1e5 100644 --- a/embedded/app/usbcan/can/lpc17xx_can.h +++ b/embedded/app/usbcan/can/lpc17xx_can.h @@ -56,41 +56,41 @@ extern "C" //---------------------------------- -#define CAN1MOD (*(uint32_t*)(CAN1_REGS_BASE+CAN_MOD_o)) -#define CAN1CMR (*(uint32_t*)(CAN1_REGS_BASE+CAN_CMR_o)) -#define CAN1GSR (*(uint32_t*)(CAN1_REGS_BASE+CAN_GSR_o)) -#define CAN1ICR (*(uint32_t*)(CAN1_REGS_BASE+CAN_ICR_o)) -#define CAN1IER (*(uint32_t*)(CAN1_REGS_BASE+CAN_IER_o)) -#define CAN1BTR (*(uint32_t*)(CAN1_REGS_BASE+CAN_BTR_o)) -#define CAN1EWL (*(uint32_t*)(CAN1_REGS_BASE+CAN_EWL_o)) -#define CAN1SR (*(uint32_t*)(CAN1_REGS_BASE+CAN_SR_o)) -#define CAN1RFS (*(uint32_t*)(CAN1_REGS_BASE+CAN_RFS_o)) -#define CAN1RID (*(uint32_t*)(CAN1_REGS_BASE+CAN_RID_o)) -#define CAN1RDA (*(uint32_t*)(CAN1_REGS_BASE+CAN_RDA_o)) -#define CAN1RDB (*(uint32_t*)(CAN1_REGS_BASE+CAN_RDB_o)) -#define CAN1TFI1 (*(uint32_t*)(CAN1_REGS_BASE+CAN_TFI1_o)) -#define CAN1TID1 (*(uint32_t*)(CAN1_REGS_BASE+CAN_TID1_o)) -#define CAN1TDA1 (*(uint32_t*)(CAN1_REGS_BASE+CAN_TDA1_o)) -#define CAN1TDB1 (*(uint32_t*)(CAN1_REGS_BASE+CAN_TDB1_o)) -#define CAN1TFI2 (*(uint32_t*)(CAN1_REGS_BASE+CAN_TFI2_o)) -#define CAN1TID2 (*(uint32_t*)(CAN1_REGS_BASE+CAN_TID2_o)) -#define CAN1TDA2 (*(uint32_t*)(CAN1_REGS_BASE+CAN_TDA2_o)) -#define CAN1TDB2 (*(uint32_t*)(CAN1_REGS_BASE+CAN_TDB2_o)) -#define CAN1TFI3 (*(uint32_t*)(CAN1_REGS_BASE+CAN_TFI3_o)) -#define CAN1TID3 (*(uint32_t*)(CAN1_REGS_BASE+CAN_TID3_o)) -#define CAN1TDA3 (*(uint32_t*)(CAN1_REGS_BASE+CAN_TDA3_o)) -#define CAN1TDB3 (*(uint32_t*)(CAN1_REGS_BASE+CAN_TDB3_o)) - - - -#define CANAF_AFMR (*(uint32_t*)(CANAF_REGS_BASE+CANAF_AFMR_o)) -#define CANAF_SFF_sa (*(uint32_t*)(CANAF_REGS_BASE+CANAF_SFF_sa_o)) -#define CANAF_SFF_GRP_sa (*(uint32_t*)(CANAF_REGS_BASE+CANAF_SFF_GRP_sa_o)) -#define CANAF_EFF_sa (*(uint32_t*)(CANAF_REGS_BASE+CANAF_EFF_sa_o)) -#define CANAF_EFF_GRP_sa (*(uint32_t*)(CANAF_REGS_BASE+CANAF_EFF_GRP_sa_o)) -#define CANAF_ENDofTable (*(uint32_t*)(CANAF_REGS_BASE+CANAF_ENDofTable_o)) -#define CANAF_LUTerrAd (*(uint32_t*)(CANAF_REGS_BASE+CANAF_LUTerrAd_o)) -#define CANAF_LUTerr (*(uint32_t*)(CANAF_REGS_BASE+CANAF_LUTerr_o)) +#define CAN1MOD (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_MOD_o)) +#define CAN1CMR (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_CMR_o)) +#define CAN1GSR (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_GSR_o)) +#define CAN1ICR (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_ICR_o)) +#define CAN1IER (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_IER_o)) +#define CAN1BTR (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_BTR_o)) +#define CAN1EWL (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_EWL_o)) +#define CAN1SR (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_SR_o)) +#define CAN1RFS (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_RFS_o)) +#define CAN1RID (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_RID_o)) +#define CAN1RDA (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_RDA_o)) +#define CAN1RDB (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_RDB_o)) +#define CAN1TFI1 (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_TFI1_o)) +#define CAN1TID1 (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_TID1_o)) +#define CAN1TDA1 (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_TDA1_o)) +#define CAN1TDB1 (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_TDB1_o)) +#define CAN1TFI2 (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_TFI2_o)) +#define CAN1TID2 (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_TID2_o)) +#define CAN1TDA2 (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_TDA2_o)) +#define CAN1TDB2 (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_TDB2_o)) +#define CAN1TFI3 (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_TFI3_o)) +#define CAN1TID3 (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_TID3_o)) +#define CAN1TDA3 (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_TDA3_o)) +#define CAN1TDB3 (*(volatile uint32_t*)(CAN1_REGS_BASE+CAN_TDB3_o)) + + + +#define CANAF_AFMR (*(volatile uint32_t*)(CANAF_REGS_BASE+CANAF_AFMR_o)) +#define CANAF_SFF_sa (*(volatile uint32_t*)(CANAF_REGS_BASE+CANAF_SFF_sa_o)) +#define CANAF_SFF_GRP_sa (*(volatile uint32_t*)(CANAF_REGS_BASE+CANAF_SFF_GRP_sa_o)) +#define CANAF_EFF_sa (*(volatile uint32_t*)(CANAF_REGS_BASE+CANAF_EFF_sa_o)) +#define CANAF_EFF_GRP_sa (*(volatile uint32_t*)(CANAF_REGS_BASE+CANAF_EFF_GRP_sa_o)) +#define CANAF_ENDofTable (*(volatile uint32_t*)(CANAF_REGS_BASE+CANAF_ENDofTable_o)) +#define CANAF_LUTerrAd (*(volatile uint32_t*)(CANAF_REGS_BASE+CANAF_LUTerrAd_o)) +#define CANAF_LUTerr (*(volatile uint32_t*)(CANAF_REGS_BASE+CANAF_LUTerr_o)) //---------------------------------- @@ -132,12 +132,6 @@ extern "C" //---------------------------------- -#define PCONP SC->PCONP -#define PCLKSEL0 SC->PCLKSEL0 - -#define PINSEL0 PINCON->PINSEL0 -#define PINMODE0 PINCON->PINMODE0 -#define PINMODE_OD0 PINCON->PINMODE_OD0 #define PCCAN1 (1<<13) // CAN Controller 1 power/clock control bit. #define PCCAN2 (1<<14) // CAN Controller 2 power/clock control bit. @@ -152,9 +146,9 @@ extern "C" // CAN1_RX_BIT a CAN1_TX_BIT jiz definovany v system_def.h -void CAN_init(uint32_t baudrate); -void CAN_send(canmsg_t* msg); -void CAN_recv(canmsg_t* msg); +void CAN_init(struct canchip_t *chip); +void CAN_send(struct canchip_t *chip, canmsg_t* msg); +void CAN_recv(struct canchip_t *chip, canmsg_t* msg); void CAN_IRQHandler(void); //---------------------------------- diff --git a/embedded/app/usbcan/lpc17xx_can.c b/embedded/app/usbcan/lpc17xx_can.c index 9defc72..5565d8c 100644 --- a/embedded/app/usbcan/lpc17xx_can.c +++ b/embedded/app/usbcan/lpc17xx_can.c @@ -1,14 +1,32 @@ #include "can/lpc17xx_can.h" static void CAN_configPin(); -static void CAN_setBusTiming(uint32_t baudrate); +static void CAN_setBusTiming(struct canchip_t *chip); extern struct canhardware_t *hardware_p; -extern volatile uint32_t cnt; //--------------------------------------------------------------------------------- //--------------------------------------------------------------------------------- + +static inline void can_write_register(struct canchip_t *chip, uint32_t data, uint32_t reg_offs){ + + uint32_t address = chip->chip_base_addr + reg_offs; + (*(volatile uint32_t*)(address)) = data; + +} + +static inline uint32_t can_read_register(struct canchip_t *chip, uint32_t reg_offs){ + + uint32_t address = chip->chip_base_addr + reg_offs; + return (*(volatile uint32_t*)(address)); + +} + +//--------------------------------------------------------------------------------- +//--------------------------------------------------------------------------------- + + // interrupt handler // for transmitting - irq when one message was transmitted (for check if another message is pending in can msg queue) // for receiving - irq when message was received and is available in Receive buffer @@ -17,22 +35,16 @@ void CAN_IRQHandler(){ uint32_t i; - i = CAN1ICR; - + struct canchip_t *chip; + chip = hardware_p->candevice[0]->chip[0]; - if(i & (CAN_ICR_TI1 | CAN_ICR_RI)){ + i = can_read_register(chip, CAN_ICR_o); - struct canchip_t *chip; - chip = hardware_p->candevice[0]->chip[0]; - + if(i & (CAN_ICR_TI1 | CAN_ICR_RI)) lpc17xx_irq_handler(0, chip); - } - if(i & CAN_ICR_DOI){ - printf("Data overrun\n"); - CAN1CMR = CAN_CMR_CDO; - } - + if(i & CAN_ICR_DOI) + can_write_register(chip, CAN_CMR_CDO, CAN_CMR_o); } @@ -73,6 +85,9 @@ int can_lmc1_init_chip_data(struct candevice_t *candev, int chipnr){ candev->chip[chipnr]->flags|= CHIP_IRQ_CUSTOM; + //debug + candev->chip[chipnr]->chip_base_addr = CAN1_REGS_BASE; + candev->chip[chipnr]->chip_data=(void *)malloc(sizeof(struct can_lmc1_chip_data)); if (candev->chip[chipnr]->chip_data==NULL) return -ENOMEM; @@ -82,20 +97,18 @@ int can_lmc1_init_chip_data(struct candevice_t *candev, int chipnr){ int can_lmc1_init_obj_data(struct canchip_t *chip, int objnr){ - //chip->msgobj[objnr]->obj_base_addr=chip->chip_base_addr+(objnr+1)*0x10; - return 0; } void can_lmc1_write_register(unsigned data, unsigned long address){ printf("UNSUPPORTED NOW\n"); - } unsigned can_lmc1_read_register(unsigned long address){ printf("UNSUPPORTED NOW\n"); + return 0; } @@ -119,7 +132,7 @@ int can_lmc1_reset(struct candevice_t *candev) int lpc17xx_chip_config(struct canchip_t *chip){ - CAN_init( (uint32_t) chip->baudrate); + CAN_init(chip); return 0; } @@ -128,7 +141,7 @@ int lpc17xx_pre_write_config(struct canchip_t *chip, struct msgobj_t *obj, struct canmsg_t *msg) { - CAN_send(msg); + CAN_send(chip, msg); return 0; } @@ -138,7 +151,7 @@ int lpc17xx_send_msg(struct canchip_t *chip, struct msgobj_t *obj, { // write transmission request - CAN1CMR = (CAN_CMR_TR | CAN_CMR_STB1); + can_write_register(chip, (CAN_CMR_TR | CAN_CMR_STB1), CAN_CMR_o); return 0; } @@ -154,8 +167,7 @@ int lpc17xx_wakeup_tx(struct canchip_t *chip, struct msgobj_t *obj) while(!can_msgobj_test_and_set_fl(obj,TX_LOCK)){ can_msgobj_clear_fl(obj,TX_REQUEST); - - if (CAN1SR & CAN_SR_TBS1){ + if (can_read_register(chip, CAN_SR_o) & CAN_SR_TBS1){ obj->tx_retry_cnt=0; lpc17xx_irq_write_handler(chip, obj); } @@ -176,8 +188,7 @@ int lpc17xx_irq_handler(int irq, struct canchip_t *chip) struct msgobj_t *obj; obj = chip->msgobj[0]; - - if(CAN1SR & CAN_SR_RBS) { + if(can_read_register(chip, CAN_SR_o) & CAN_SR_RBS) { lpc17xx_read(chip,obj); obj->ret = 0; } @@ -192,7 +203,7 @@ int lpc17xx_irq_handler(int irq, struct canchip_t *chip) obj->ret=0; can_msgobj_clear_fl(obj,TX_REQUEST); - if (CAN1SR & CAN_SR_TBS1){ + if (can_read_register(chip, CAN_SR_o) & CAN_SR_TBS1){ obj->tx_retry_cnt=0; lpc17xx_irq_write_handler(chip, obj); } @@ -252,7 +263,7 @@ void lpc17xx_irq_write_handler(struct canchip_t *chip, struct msgobj_t *obj) void lpc17xx_read(struct canchip_t *chip, struct msgobj_t *obj) { - CAN_recv(&obj->rx_msg); + CAN_recv(chip, &obj->rx_msg); // fill CAN message timestamp can_filltimestamp(&obj->rx_msg.timestamp); @@ -260,16 +271,16 @@ void lpc17xx_read(struct canchip_t *chip, struct msgobj_t *obj) { canque_filter_msg2edges(obj->qends, &obj->rx_msg); // release Receive buffer - CAN1CMR = CAN_CMR_RRB; + can_write_register(chip, CAN_CMR_RRB, CAN_CMR_o); } int lpc17xx_fill_chipspecops(struct canchip_t *chip){ chip->max_objects=1; - chip->baudrate=1000000; - + lpc17xx_register(chip->chipspecops); + return 0; } @@ -318,21 +329,21 @@ static void CAN_configPin(){ uint8_t pinmode_od0_val = 0; - pinsel0 = PINSEL0; + pinsel0 = PINCON->PINSEL0; pinsel0 &= ~CAN1_RX_MASK; pinsel0 &= ~CAN1_TX_MASK; pinsel0 |= __val2mfld(CAN1_RX_MASK, pinsel0_val); pinsel0 |= __val2mfld(CAN1_TX_MASK, pinsel0_val); - PINSEL0 = pinsel0; + PINCON->PINSEL0 = pinsel0; - pinmode0 = PINMODE0; + pinmode0 = PINCON->PINMODE0; pinmode0 &= ~CAN1_RX_MASK; pinmode0 &= ~CAN1_TX_MASK; pinmode0 |= __val2mfld(CAN1_RX_MASK, pinmode0_val); pinmode0 |= __val2mfld(CAN1_TX_MASK, pinmode0_val); - PINMODE0 = pinmode0; + PINCON->PINMODE0 = pinmode0; - pinmode_od0 = PINMODE_OD0; + pinmode_od0 = PINCON->PINMODE_OD0; if (pinmode_od0_val){ pinmode_od0 |= CAN1_RX_BIT; pinmode_od0 |= CAN1_TX_BIT; @@ -341,105 +352,97 @@ static void CAN_configPin(){ pinmode_od0 &= ~CAN1_RX_BIT; pinmode_od0 &= ~CAN1_TX_BIT; } - PINMODE_OD0 = pinmode_od0; + PINCON->PINMODE_OD0 = pinmode_od0; } -void CAN_recv(canmsg_t* msg){ - -/* - msg->id = ++cnt; - msg->data[0] = 0xAA; - msg->data[1] = 0xBB; - msg->length = 2; -*/ - +void CAN_recv(struct canchip_t *chip, canmsg_t* msg){ - uint32_t data; + volatile uint32_t data; uint32_t i; - // only for debug - cnt++; - // read data lenght - msg->length = ((CAN1RFS)>>16) & 0xF; + msg->length = (can_read_register(chip, CAN_RFS_o)>>16) & 0xF; // read identifier - msg->id = CAN1RID; + msg->id = can_read_register(chip, CAN_RID_o); // EXT frame - if(CAN1RFS & CAN_RFS_EXT) + if(can_read_register(chip, CAN_RFS_o) & CAN_RFS_EXT) msg->flags |= MSG_EXT; else msg->flags &= ~MSG_EXT; // RTR frame - if(CAN1RFS & CAN_RFS_RTR) + if(can_read_register(chip, CAN_RFS_o) & CAN_RFS_RTR) msg->flags |= MSG_RTR; else msg->flags &= ~MSG_RTR; - // read data - data = CAN1RDA; + // read data + data = can_read_register(chip, CAN_RDA_o); for(i=0; i<4; i++) msg->data[i] = (data>>(i*8)) & 0xFF; - data = CAN1RDB; + data = can_read_register(chip, CAN_RDB_o); for(i=4; i<8; i++) msg->data[i] = (data>>((i-4)*8)) & 0xFF; } -void CAN_send(canmsg_t* msg){ +void CAN_send(struct canchip_t *chip, canmsg_t* msg){ - uint32_t data; + volatile uint32_t data; + volatile uint32_t can_tfi1; uint32_t i; // check status of TB1 - while (!(CAN1SR & CAN_SR_TBS1)){} - - CAN1TFI1 &= ~0x000F0000; - CAN1TFI1 |= (msg->length)<<16; + while (!(can_read_register(chip, CAN_SR_o) & CAN_SR_TBS1)){} + + can_tfi1 = can_read_register(chip, CAN_TFI1_o); + + can_tfi1 &= ~0x000F0000; + can_tfi1 |= (msg->length)<<16; // EXT frame if(msg->flags & MSG_EXT) - CAN1TFI1 |= CAN_TFI1_EXT; + can_tfi1 |= CAN_TFI1_EXT; else - CAN1TFI1 &= ~CAN_TFI1_EXT; + can_tfi1 &= ~CAN_TFI1_EXT; // RTR frame if(msg->flags & MSG_RTR) - CAN1TFI1 |= CAN_TFI1_RTR; + can_tfi1 |= CAN_TFI1_RTR; else - CAN1TFI1 &= ~CAN_TFI1_RTR; + can_tfi1 &= ~CAN_TFI1_RTR; + + can_write_register(chip, can_tfi1, CAN_TFI1_o); + // write CAN ID - CAN1TID1 = msg->id; + can_write_register(chip, msg->id, CAN_TID1_o); - // write first 4 data bytes + // write first 4 data bytes data=0; for(i=0; i<4; i++) data |= (msg->data[i])<<(i*8); - CAN1TDA1 = data; + can_write_register(chip, data, CAN_TDA1_o); // write second 4 data bytes - data=0; for(i=4; i<8; i++) data |= (msg->data[i])<<((i-4)*8); - CAN1TDB1 = data; - - // write transmission request - //CAN1CMR = 0x21; + + can_write_register(chip, data, CAN_TDB1_o); } -void CAN_setBusTiming(uint32_t baudrate){ +void CAN_setBusTiming(struct canchip_t *chip){ uint32_t PCLK_CAN; uint32_t res; @@ -458,7 +461,7 @@ void CAN_setBusTiming(uint32_t baudrate){ SJW = 3; // get clock divide for CAN1 - div = __mfld2val(PCLK_CAN1_MASK, PCLKSEL0); + div = __mfld2val(PCLK_CAN1_MASK, SC->PCLKSEL0); switch(div){ case 0: div = 4; @@ -478,7 +481,7 @@ void CAN_setBusTiming(uint32_t baudrate){ PCLK_CAN = system_frequency / div; - res = PCLK_CAN / baudrate; + res = PCLK_CAN / (chip->baudrate); // calculation of tq_numb - number of time quantum (must be in <8,25>) @@ -504,57 +507,58 @@ void CAN_setBusTiming(uint32_t baudrate){ } } - CAN1BTR = ((SAM<<23)|(TSEG2<<20)|(TSEG1<<16)|(SJW<<14)|(BRP<<0)); + can_write_register(chip, ((SAM<<23)|(TSEG2<<20)|(TSEG1<<16)|(SJW<<14)|(BRP<<0)), CAN_BTR_o); } -void CAN_init(uint32_t baudrate) -{ +void CAN_init(struct canchip_t *chip){ + uint32_t tmp; uint32_t pclksel0; uint32_t val; uint32_t i; - printf("CAN INIT, baudrate: %d\n", baudrate); + printf("CAN INIT, baudrate: %d\n", chip->baudrate); // configure CAN1 pins CAN_configPin(); // turn on power and clock for CAN1 - PCONP |= PCCAN1; + SC->PCONP |= PCCAN1; // set clock divide for CAN1 val = 0x00; // 00 PCLK_peripheral = CCLK/4 - pclksel0 = PCLKSEL0; + pclksel0 = SC->PCLKSEL0; pclksel0 &= ~PCLK_CAN1_MASK; pclksel0 &= ~PCLK_CAN2_MASK; pclksel0 &= ~PCLK_ACF_MASK; pclksel0 |= __val2mfld(PCLK_CAN1_MASK, val); pclksel0 |= __val2mfld(PCLK_CAN2_MASK, val); pclksel0 |= __val2mfld(PCLK_ACF_MASK, val); - PCLKSEL0 = pclksel0; + SC->PCLKSEL0 = pclksel0; // enter reset mode - CAN1MOD = 1; + can_write_register(chip, 1, CAN_MOD_o); // disable all CAN interrupts - CAN1IER = 0; + can_write_register(chip, 0, CAN_IER_o); // reset value of Global Status Register (global controller status and error counters) - CAN1GSR = 0x3C; + can_write_register(chip, 0x3C, CAN_GSR_o); // request command to release Rx, Tx buffer and clear data overrun - CAN1CMR = (CAN_CMR_AT | CAN_CMR_RRB | CAN_CMR_CDO); + can_write_register(chip, (CAN_CMR_AT | CAN_CMR_RRB | CAN_CMR_CDO), CAN_CMR_o); // read to clear interrupt pending in Interrupt Capture Register - tmp = CAN1ICR; + tmp = can_read_register(chip, CAN_ICR_o); // set bus timing - CAN_setBusTiming(baudrate); + CAN_setBusTiming(chip); // return to normal operating - CAN1MOD = 0; + can_write_register(chip, 0, CAN_MOD_o); + //-------------------------- @@ -572,18 +576,16 @@ void CAN_init(uint32_t baudrate) CANAF_EFF_GRP_sa = 0x00; CANAF_ENDofTable = 0x00; - // Acceptance Filter Bypass Mode - all messages accepted CANAF_AFMR = 0x02; //-------------------------- + // enable interrupt after transmit - CAN1IER |= CAN_IER_TIE1; // enable receive interrupt - CAN1IER |= CAN_IER_RIE; // enable data overrun interrupt - CAN1IER |= CAN_IER_DOIE; + can_write_register(chip, (CAN_IER_TIE1 | CAN_IER_RIE | CAN_IER_DOIE), CAN_IER_o); // enable CAN interrupt NVIC_EnableIRQ(CAN_IRQn); diff --git a/embedded/app/usbcan/main.c b/embedded/app/usbcan/main.c index ad7fe36..41062e3 100644 --- a/embedded/app/usbcan/main.c +++ b/embedded/app/usbcan/main.c @@ -67,9 +67,8 @@ #include "./usb/usb_defs.h" #include "./usb/usb_vend.h" -// DEBUG CAN + #include "can/lpc17xx_can.h" -// DEBUG CAN - end #define MASK_EP1RX 0x01 #define MASK_EP1TX 0x02 @@ -109,7 +108,6 @@ typedef void (*FNC)(); //function ptr /*********************************************************************** * global variables ***********************************************************************/ -volatile uint32_t cnt; usb_device_t usb_device; @@ -209,8 +207,6 @@ int main(void) int chipnr,bd; int i,size,m=0; - cnt=0; - CANMSG("Starting USBCAN module firmware...\n"); // volatile int i=0; @@ -230,15 +226,13 @@ int main(void) sys_err(); } -// !!! DEBUG - transmitting and receiving CAN messages with usage of queue system from LinCAN - first version! - //*********************************************************************** // * CAN device initialization - device side (adapted from LinCAN setup.c) // *********************************************************************** // DEBUG - //can_init(); // only for successful compiling (defined in can_lpcbusemu.c) + //can_init(); // useless with lpc17xx (defined in can_lpcbusemu.c) DEBUGMSG("Initiating CAN device initialization\n"); @@ -270,8 +264,8 @@ int main(void) // DEBUG - //ul_usb1_register(candev->hwspecops); // only for successful compiling (defined in ul_usb1.c) - // register for another board + //ul_usb1_register(candev->hwspecops); //(defined in ul_usb1.c) + // register for another board: can_lmc1_register(candev->hwspecops); @@ -434,14 +428,10 @@ int main(void) usb_check_events(&usb_device); usb_control_response(&usb_device); -// DEBUG +// useless with lpc17xx: // if (!(IO0PIN&P0_SJA1000_INT_PIN)) //INT PIN is inverted // chip->chipspecops->irq_handler(0,chip); -// if (CAN1SR & CAN_SR_RBS) -// chip->chipspecops->irq_handler(0,chip); - - if (usb_device.ep_events & MASK_EP1RX) { //EP1RX - data waiting to receive if (canque_get_inslot(qends, &qedge, &slot, 0)>=0){ //Free slot obtained @@ -475,7 +465,7 @@ int main(void) slot->msg=canmsg; canque_put_inslot(qends, qedge, slot); - //CAN_send(&canmsg); + } else canque_abort_inslot(qends,qedge,slot); @@ -534,8 +524,6 @@ int main(void) } usb_udev_write_endpoint(&eps[1],ep1_tx_buff,16); - //printf("ID: %d\n", msgid); - canque_free_outslot(qends, qedge, slot); timer_tx_off=50; //rozsviceni diod pri vysilani CLR_OUT_PIN(LED_PORT,LED1_BIT); -- 2.39.2