MPC5200 with MSCAN - Beta version
authorMartin Petera <martin.petera@gmail.com>
Tue, 5 Jan 2010 05:10:57 +0000 (06:10 +0100)
committerMartin Petera <martin.petera@gmail.com>
Tue, 5 Jan 2010 05:10:57 +0000 (06:10 +0100)
lincan/include/mpc5200.h
lincan/include/mscan.h
lincan/src/mpc5200.c
lincan/src/mscan.c

index d63ffbd..79dbc30 100644 (file)
 /* Publication of enhanced or derived LinCAN files is required although.  */
 /**************************************************************************/
 
+#ifndef LINCAN_MPC5200_H
+#define LINCAN_MPC5200_H
+
+/* Change this to distinguish between original MIDAM board and CTU daughterboard aka RYU */
+#define MSCAN_MIDAM    2
+
+
+/* MPC5200 has two CAN controlers
+ * however MIDAM board uses only one controler
+ * RYU board uses both
+ */
+#ifdef MSCAN_MIDAM
+       #define NR_MSCAN        1
+#else
+       #ifdef MSCAN_RYU
+               #define NR_MSCAN        2
+       #else
+               #define NR_MSCAN        0
+       #endif /* MSCAN_RYU */
+#endif /* MSCAN_MIDAM */
+
+
 #define NR_82527       0
 #define NR_SJA1000     0
-#define NR_MSCAN       2                               /* this isnt used in Lincan core */
-#define NR_ALL         2
+#define NR_ALL         (NR_82527 + NR_SJA1000 + NR_MSCAN)
 
-#define MPC5200_CAN_IRQ                17              /* IRQ 17, 18 according to shark.dts */
-#define MPC5200_CAN_CHIP_OFFSET                0x80
 
+/* IRQ is read from OpenFirmware nodes */
+
+#define MPC5200_CAN_CHIP_OFFSET                0x80
 #define IO_RANGE                       0x80
 
 
 /* Clock frequency - used for baudrate */
-// TODO: zjistit frekvence IPB a SYS_XTAL
-#define MPC5200_SHARK_SYS_XTAL_FREQ    32768000        /* 32768k xtal */
-#define MPC5200_SHARK_IPB_FREQ         3300000         /* derived from XTAL by dividing */
+#define MPC5200_SHARK_SYS_XTAL_FREQ    396000000       /* 396 MHz */
+#define MPC5200_SHARK_IPB_FREQ         132000000       /* derived from XTAL by dividing: 132 MHz */
+/* got from /proc/device-tree/cpus/PowerPC,5200@0/ bus-frequency and system-frequency */
 
 /* Determine which clock source to use */
 /* 0 - use IP Bus clock                */
 /* 1 - use SYS_XTAL_IN frequency       */
-#define MPC5200_CLKSRC         1               
+#define MPC5200_CLKSRC         1
 
-#ifndef MPC5200_CLKSRC
-       #define MPC5200_CLK_FREQ        MPC5200_SHARK_SYS_XTAL_FREQ
+#if MPC5200_CLKSRC
+       #define MPC5200_CLK_FREQ        (MPC5200_SHARK_SYS_XTAL_FREQ/12)        /* 33MHz */
 #else
-       #define MPC5200_CLK_FREQ        MPC5200_SHARK_IPB_FREQ
+       #define MPC5200_CLK_FREQ        (MPC5200_SHARK_IPB_FREQ/4)              /* 33MHz */
 #endif
 
+/* ----------- Debugging options for MPC5200 ----------- */
+
+#define MPC5200_DBG 0
 
+#if MPC5200_DBG
+       /* standard LinCAN core debug - used only for MPC5200 driver part */
+       #define DEBUGMSG(fmt,args...) can_printk(KERN_ERR "lincan (debug): " fmt,##args)
+#endif /* MPC5200_DBG */
 
-/* static CAN_DEFINE_SPINLOCK(mpc5200_port_lock); */
 
 int mpc5200_request_io(struct candevice_t *candev);
 int mpc5200_release_io(struct candevice_t *candev);
@@ -74,3 +102,4 @@ int mpc5200_program_irq(struct candevice_t *candev);
 void mpc5200_write_register(unsigned data, can_ioptr_t address);
 unsigned mpc5200_read_register(can_ioptr_t address);
 
+#endif /* LINCAN_MPC5200_H */
index 0daf98c..a85a291 100644 (file)
 /* Publication of enhanced or derived LinCAN files is required although.  */
 /**************************************************************************/
 
+#ifndef LINCAN_MSCAN_H
+#define LINCAN_MSCAN_H
 
+
+
+#define MPC5200_DBG 0
+
+/* Debug - coarse approach */
+#if MPC5200_DBG
+       /* standard LinCAN core debug - used only for MPC5200 driver part */
+       #define DEBUGMSG(fmt,args...) can_printk(KERN_ERR "lincan (debug): " fmt,##args)
+
+       /* dump specific parts of chip memory */        
+       #define DUMPREGS(canchip) dump_regs(canchip)
+       #define DUMPBUFF(canchip, offset) dump_buff(canchip, offset)
+       #define DUMPFLT(canchip) dump_filter(canchip)
+
+       /* Debug Tx Rx operations */
+       #define DEBUGTX(fmt,args...) can_printk(KERN_ERR "lincan (debug): " fmt,##args)
+       #define DEBUGRX(fmt,args...) can_printk(KERN_ERR "lincan (debug): " fmt,##args)
+#else
+       #define DUMPREGS(canchip)
+       #define DUMPBUFF(canchip, offset)
+       #define DUMPFLT(canchip)
+       #define DEBUGTX(fmt,args...)
+       #define DEBUGRX(fmt,args...)
+#endif /* MPC5200_DBG */
+
+
+
+
+/* MSCAN register size */
 #define reg_t uint8_t
 
+
+/* Determine which clock source to use */
+/* 0 - use IP Bus clock                */
+/* 1 - use SYS_XTAL_IN frequency       */
+#define MPC5200_CLKSRC         1
+
+#if MPC5200_CLKSRC
+       #define MPC5200_CLK_FREQ        (MPC5200_SHARK_SYS_XTAL_FREQ/12)        /* 33MHz */
+#else
+       #define MPC5200_CLK_FREQ        (MPC5200_SHARK_IPB_FREQ/4)              /* 33MHz */
+#endif
+
+
 int mscan_chip_config(struct canchip_t *chip);
 int mscan_enable_configuration(struct canchip_t *chip);
 int mscan_disable_configuration(struct canchip_t *chip);
@@ -101,7 +145,7 @@ extern inline uint16_t can_read_reg_w(const struct canchip_t *pchip, unsigned re
 #define MSCAN_RIER     0x09            /* Receiver Interrupt Enable Register */
 #define MSCAN_TFLG     0x0c            /* Transmitter Flag Register */
 #define MSCAN_TIER     0x0d            /* Transmitter Interrupt Enable Register */
-#define MSCAN_TARG     0x10            /* Transmitter Message Abort Request Register */
+#define MSCAN_TARQ     0x10            /* Transmitter Message Abort Request Register */
 #define MSCAN_TAAK     0x11            /* Transmitter Message Abort Acknowledge Register */
 #define MSCAN_TBSEL    0x14            /* Transmitter Buffer Selection */
 #define MSCAN_IDAC     0x15            /* Identifier Acceptance Control Register */
@@ -183,7 +227,7 @@ enum mscan_rflg {
        MSCAN_RFLG_WUPIF        = 1 << 7,               /* WakeUp Interrupt Flag - rw */
        MSCAN_RFLG_CSCIF        = 1 << 6,               /* CAN Status Change Interrupt Flag - rw */
        MSCAN_RFLG_RSTAT        = 0x30,                 /* Receiver Status Bits: 0-RxOK, 1-RxWRN, 2-RxERR, 3-BusOff - ro */
-       MSCAN_RFLG_TSTAT        = 0x0a,                 /* Transmitter Status Bits: 0-TxOK, 1-TxWRN, 2-TxErr, 3-BusOff - ro */
+       MSCAN_RFLG_TSTAT        = 0x0c,                 /* Transmitter Status Bits: 0-TxOK, 1-TxWRN, 2-TxErr, 3-BusOff - ro */
        MSCAN_RFLG_OVRIF        = 1 << 1,               /* Overrun Interrupt Flag - rw */
        MSCAN_RFLG_RXF          = 1 << 0,               /* Receive Buffer Full - rw */
 };
@@ -192,7 +236,7 @@ enum mscan_rier {
        MSCAN_RIER_WUPIE        = 1 << 7,               /* WakeUp Interrupt Enable - rw */
        MSCAN_RIER_CSCIE        = 1 << 6,               /* CAN Status Change Interrupt Enable - rw */
        MSCAN_RIER_RSTATE       = 0x30,                 /* Receiver Status Change Enable: 0-Never, 1-BusOff, 2-BusOff+RxErr, 3-All - rw */
-       MSCAN_RIER_TSTATE       = 0x0a,                 /* Transmitter Status Change Enable: 0-Never, 1-BusOff, 2-BusOff+TxErr, 3-All - rw */
+       MSCAN_RIER_TSTATE       = 0x0c,                 /* Transmitter Status Change Enable: 0-Never, 1-BusOff, 2-BusOff+TxErr, 3-All - rw */
        MSCAN_RIER_OVRIE        = 1 << 1,               /* Overrun Interrupt Enable - rw */
        MSCAN_RIER_RXFIE        = 1 << 0,               /* Receive Buffer Full Interrupt Enable - rw */
 };
@@ -281,3 +325,41 @@ struct mscan_msg_buffer{
        uint8_t         timestamp_h;            /* local timestamp - High byte - Read Only */
        uint8_t         timestamp_l;            /* local timestamp - Low byte - Read Only */
 };
+
+/* structure for memory layout of acceptance
+ * filter registers
+ */
+struct mscan_flt_regs{
+       uint8_t         acp_id_0;
+       uint8_t         acp_id_1;
+       uint16_t        spacer_0;
+
+       uint8_t         acp_id_2;
+       uint8_t         acp_id_3;
+       uint16_t        spacer_1;
+
+       uint8_t         acp_mask_0;
+       uint8_t         acp_mask_1;
+       uint16_t        spacer_2;
+
+       uint8_t         acp_mask_2;
+       uint8_t         acp_mask_3;
+       uint16_t        spacer_3;
+
+       uint8_t         acp_id_4;
+       uint8_t         acp_id_5;
+       uint16_t        spacer_4;
+
+       uint8_t         acp_id_6;
+       uint8_t         acp_id_7;
+       uint16_t        spacer_5;
+
+       uint8_t         acp_mask_4;
+       uint8_t         acp_mask_5;
+       uint16_t        spacer_6;
+
+       uint8_t         acp_mask_6;
+       uint8_t         acp_mask_7;
+};
+
+#endif /* LINCAN_MSCAN_H */
index 10d23a1..2e159cc 100644 (file)
 #include "../include/main.h"
 #include "../include/mpc5200.h"
 #include "../include/mscan.h"
+#include "linux/of.h"
+#include "linux/of_platform.h"
+
+int mpc5200_init_device_node(struct canchip_t * chip, struct device_node * devnode);
+
+/* in time when release_io should take place, we dont know the
+ * base addresses that has to be free.
+ * This variable keeps those addresses
+ */
+unsigned long * chips_addr;
+
 
 int mpc5200_request_io(struct candevice_t *candev)
 {
-       if (!can_request_io_region(candev->io_addr, candev->nr_all_chips * IO_RANGE, DEVICE_NAME)) {
-               CANMSG("Unable to open port: 0x%lx\n",candev->io_addr);
-               return -ENODEV;
+       int chipnr;
+       struct device_node * dn;
+       const char * of_devname = "can";
+
+       /* Workaround for OpenFormware */
+
+       /* Select device by chipnr:
+        *  first dev is @ 900
+        *  second dev is @ 980
+        * 
+        * There are only two devices on MPC5200 */
+
+       /* initialize internal address storage */
+       chips_addr = (unsigned long*)kmalloc(candev->nr_all_chips * sizeof(unsigned long), GFP_KERNEL);
+
+       /* DEBUGMSG("Looking for device node '%s'\n", of_devname); */
+
+       chipnr = 0;
+       for_each_node_by_name(dn, of_devname)
+       {
+               /* DEBUGMSG(" got OF node (%s)...\n", of_devname); */
+
+               if (!of_device_is_available(dn))
+               {
+                       DEBUGMSG("\tdevice not available\n");
+                       continue;
+               }
+
+               
+               if (mpc5200_init_device_node(candev->chip[chipnr], dn))
+                       return -ENODEV;
+
+               /* fill received address to internal storage */
+               chips_addr[chipnr] = (unsigned long)candev->chip[chipnr]->chip_base_addr;
+
+               /* original MIDAM Shark board use only one CAN chip - this handles similar cases */
+               if (++chipnr >= candev->nr_all_chips)
+                       break;
        }
 
-       DEBUGMSG("Registered IO-memory: 0x%lx - 0x%lx\n", candev->io_addr, candev->io_addr + candev->nr_all_chips * IO_RANGE - 1);
+
+       DEBUGMSG("Succesfully mapped %d can devices\n", chipnr);
        return 0;
 }
 
 int mpc5200_release_io(struct candevice_t *candev)
 {
-       can_release_io_region(candev->io_addr, candev->nr_all_chips * IO_RANGE);
+       int i;
 
+       /*can_release_io_region(candev->io_addr, candev->nr_all_chips * IO_RANGE);*/
+
+       /* free all chips memorey space - using internal address storage */
+       for (i = 0; i < candev->nr_all_chips; i++)
+               iounmap((void*)chips_addr[i]);
+       
        return 0;
 }
 
@@ -65,7 +118,8 @@ int mpc5200_reset(struct candevice_t *candev)
        for (i = 0; i < candev->nr_all_chips; i++)
        {
            /* !!! Assuming this card has ONLY MSCAN chips !!! */
-           if (mscan_reset_chip(candev->chip[i])) return -ENODEV;
+           if (mscan_reset_chip(candev->chip[i]))
+               return -ENODEV;
        }
 
        return 0;
@@ -85,11 +139,15 @@ int mpc5200_init_chip_data(struct candevice_t *candev, int chipnr)
 {
        mscan_fill_chipspecops(candev->chip[chipnr]);
 
-       candev->chip[chipnr]->chip_base_addr = can_ioport2ioptr(candev->io_addr) + chipnr * MPC5200_CAN_CHIP_OFFSET; /* one chip with 2 interfaces */
        candev->chip[chipnr]->clock = MPC5200_CLK_FREQ;
-       candev->chip[chipnr]->chip_irq = MPC5200_CAN_IRQ + chipnr;
        candev->chip[chipnr]->hostdevice = candev;
 
+/*
+       candev->chip[chipnr]->chip_base_addr = can_ioport2ioptr(candev->io_addr) + chipnr * MPC5200_CAN_CHIP_OFFSET;  one chip with 2 interfaces
+       candev->chip[chipnr]->chip_irq = MPC5200_CAN_IRQ + chipnr;
+*/
+       DEBUGMSG("mpc5200_init_chip_data\n");
+
        return 0;
 }
 
@@ -111,14 +169,14 @@ int mpc5200_program_irq(struct candevice_t *candev)
 void mpc5200_write_register(unsigned data, can_ioptr_t address)
 {
        /* address is an absolute address */
-       DEBUGMSG("write register\n");
+       /* DEBUGMSG("write register\n"); */
        writeb(data, address);          /* regs in PowerPC (5200) are one-byte length */
 }
 
 unsigned mpc5200_read_register(can_ioptr_t address)
 {
        /* address is an absolute address */
-       DEBUGMSG("read register\n");
+       /* DEBUGMSG("read register\n"); */
        return readb(address);          /* regs in PowerPC (5200) are one-byte length */
 }
 
@@ -130,8 +188,31 @@ int mpc5200_register(struct hwspecops_t *hwspecops)
        hwspecops->init_hw_data = mpc5200_init_hw_data;
        hwspecops->init_chip_data = mpc5200_init_chip_data;
        hwspecops->init_obj_data = mpc5200_init_obj_data;
-       hwspecops->program_irq = mpc5200_program_irq;
+       hwspecops->program_irq = NULL; /* mpc5200_program_irq; */
        hwspecops->write_register = mpc5200_write_register;
        hwspecops->read_register = mpc5200_read_register;
        return 0;
 }
+
+
+int mpc5200_init_device_node(struct canchip_t * chip, struct device_node * devnode)
+{
+       chip->chip_base_addr = of_iomap(devnode, 0);
+       if (!chip->chip_base_addr)
+       {
+               DEBUGMSG("\tcannot map IO - of_iomap\n");
+               return -ENODEV;
+       }
+
+       chip->chip_irq = irq_of_parse_and_map(devnode, 0);
+       if (!chip->chip_irq)
+       {
+               DEBUGMSG("\tcannot map IRQ\n");
+               iounmap(chip->chip_base_addr);
+               return -ENODEV;
+       }
+
+       DEBUGMSG("Bound to io-addr: 0x%08x IRQ: %d\n", (unsigned int)chip->chip_base_addr, chip->chip_irq);
+       
+       return 0;
+}
index 65b648a..3456ffc 100644 (file)
 #include "../include/can_sysdep.h"
 #include "../include/main.h"
 #include "../include/mscan.h"
-#include "../include/mpc5200.h"
-
-#define myDEBUG 1              /* enable debug for MPC5200 with MSCAN only */
-
-#if myDEBUG
-       #define DEBUGMSG(fmt,args...) can_printk(KERN_ERR "lincan (debug): " fmt,##args)
-#endif
+//#include "../include/mpc5200.h"
 
 
 #define MSCAN_MAX_TRANSMIT_WAIT_LOOPS 20
-#define MSCAN_MAX_SETTING_WAIT_LOOPS 25                /* maximal loop count while checking Chip reply to action */
+#define MSCAN_MAX_SETTING_WAIT_LOOPS 25                /* maximal loop count while checking Chip reply to action ~ 5 ms */
 #define MSCAN_MAX_IRQ_WAIT_LOOPS 25
 
+CAN_DEFINE_SPINLOCK(mscan_prewr_lock);
 
 void mscan_irq_read_handler(struct canchip_t *chip, struct msgobj_t *obj);
 void mscan_irq_write_handler(struct canchip_t *chip, struct msgobj_t *obj);
@@ -58,7 +53,7 @@ int mscan_clear_irq_flags(struct canchip_t *chip);
 void mscan_setup_ctrl_regs(struct canmsg_t * msg, uint16_t * ctrl0, uint16_t * ctrl1, uint16_t * ctrl2);
 
 void mscan_msg_from_rxbuffer(struct canchip_t * chip, struct canmsg_t * msg);
-void mscan_setup_txbuffer(struct canchip_t * chip, struct canmsg_t * msg, int buffer);
+void mscan_setup_txbuffer(struct canchip_t * chip, struct canmsg_t * msg);
 
 void mscan_notifyRXends(struct msgobj_t * obj, int what);
 int mscan_check_txbuff_stat(struct canchip_t *chip, int buffer);
@@ -68,21 +63,107 @@ static int mscan_update_samplept(int sampl_pt, int tseg, int *tseg1, int *tseg2)
 static int mscan_init_mode_active(struct canchip_t *chip);
 static int mscan_sleep_mode_active(struct canchip_t *chip);
 
+static int mscan_enter_init_mode(struct canchip_t * chip);
+static int mscan_enter_sleep_mode(struct canchip_t * chip);
+
 static reg_t mscan_get_flags(struct canchip_t * chip, reg_t flags, unsigned reg_addr);
 static void mscan_set_flags(struct canchip_t * chip, reg_t flags, unsigned reg_addr);
 static void mscan_clear_flags(struct canchip_t * chip, reg_t flags, unsigned reg_addr);
+static uint16_t mscan_get_irq_flags(struct canchip_t * chip);
 
 static void mscan_clear_buffer(struct canchip_t * chip, unsigned start_addr);
+static int mscan_abort_msg(struct canchip_t * chip, reg_t buffer_mask);
+
+#if MPC5200_DBG
+       static void dump_regs(struct canchip_t * chip)
+       {
+               CANMSG("MSCAN reg dump\n");
+               CANMSG("CTL0 0x%02x\tCTL1 0x%02x\tBTR0 0x%02x\tBTR1 0x%02x\n",
+                       mscan_get_flags(chip, 0xff, MSCAN_CTL0),
+                       mscan_get_flags(chip, 0xff, MSCAN_CTL1),
+                       mscan_get_flags(chip, 0xff, MSCAN_BTR0),
+                       mscan_get_flags(chip, 0xff, MSCAN_BTR1));
+       
+               CANMSG("RFLG 0x%02x\tRIER 0x%02x\tTFLG 0x%02x\tTIER 0x%02x\n",
+                       mscan_get_flags(chip, 0xff, MSCAN_RFLG),
+                       mscan_get_flags(chip, 0xff, MSCAN_RIER),
+                       mscan_get_flags(chip, 0xff, MSCAN_TFLG),
+                       mscan_get_flags(chip, 0xff, MSCAN_TIER));
+       
+               CANMSG("TARQ 0x%02x\tTAAK 0x%02x\tTBSEL 0x%02x\tIDAC 0x%02x\n",
+                       mscan_get_flags(chip, 0xff, MSCAN_TARQ),
+                       mscan_get_flags(chip, 0xff, MSCAN_TAAK),
+                       mscan_get_flags(chip, 0xff, MSCAN_TBSEL),
+                       mscan_get_flags(chip, 0xff, MSCAN_IDAC));
+       
+               CANMSG("RXERR 0x%02x\tTXERR 0x%02x\n",
+                       mscan_get_flags(chip, 0xff, MSCAN_RXERR),
+                       mscan_get_flags(chip, 0xff, MSCAN_TXERR));
+       
+       }
+       
+       static void dump_buff(struct canchip_t * chip, unsigned offset_addr)
+       {
+               volatile struct mscan_msg_buffer * buff = (struct mscan_msg_buffer *)(chip->chip_base_addr + offset_addr);
+       
+               CANMSG("MSCAN buffer dump\n");
+               
+               /* structural access */
+               CANMSG("Data0 0x%02x Data1 0x%02x Data2 0x%02x Data3 0x%02x Data4 0x%02x Data5 0x%02x Data6 0x%02x Data7 0x%02x\n",
+                       buff->data_0, buff->data_1, buff->data_2, buff->data_3, buff->data_4, buff->data_5, buff->data_6, buff->data_7);
+               CANMSG("Data Len %d\tPriority 0x%02x\n",
+                       buff->data_len, buff->local_prio);
+               CANMSG("IDR0 0x%02x\tIDR1 0x%02x\tIDR2 0x%02x\tIDR3 0x%02x\n",
+                       buff->id_0, buff->id_1, buff->id_2, buff->id_3);
+       
+       }
+
+       static void dump_filter(struct canchip_t * chip)
+       {
+               volatile struct mscan_flt_regs * flt = (struct mscan_flt_regs *)(chip->chip_base_addr + MSCAN_IDAR0);
+
+               CANMSG("MSCAN Acceptance filter dump\n");
+
+               CANMSG("IDAC 0x%02x\n", mscan_get_flags(chip, MSCAN_IDAC_IDAM | MSCAN_IDAC_IDHIT, MSCAN_IDAC));
+
+               CANMSG("IDAR0 0x%02x\tIDAR1 0x%02x\tIDAR2 0x%02x\tIDAR3 0x%02x\n",
+                       flt->acp_id_0, flt->acp_id_1, flt->acp_id_2, flt->acp_id_3);
+               CANMSG("IDMR0 0x%02x\tIDMR1 0x%02x\tIDMR2 0x%02x\tIDMR3 0x%02x\n",
+                       flt->acp_mask_0, flt->acp_mask_1, flt->acp_mask_2, flt->acp_mask_3);
+
+               CANMSG("IDAR4 0x%02x\tIDAR5 0x%02x\tIDAR6 0x%02x\tIDAR7 0x%02x\n",
+                       flt->acp_id_4, flt->acp_id_5, flt->acp_id_6, flt->acp_id_7);
+               CANMSG("IDMR4 0x%02x\tIDMR5 0x%02x\tIDMR6 0x%02x\tIDMR7 0x%02x\n",
+                       flt->acp_mask_4, flt->acp_mask_5, flt->acp_mask_6, flt->acp_mask_7);
+       }
+#endif /* MPC5200_DBG */
+
+
+/* macro for standardized CAN Bus Status change report */
+#define MSCAN_STAT_CHANGE(msg,idx) CANMSG("MSCAN chip %d %s\n", idx, msg) 
 
 /* Enable folowing IRQs
- * MSCAN_TIER_TXE - Transmit Empty Interrupt
+ * MSCAN_TIER_TXE - Transmit Empty Interrupt - Set and Cleared during TX proccess not during init
  * MSCAN_RIER_RXFIE - Receive Buffer Full
+ * MSCAN_RIER_RSTATE - Receiver Status Change Enable: 0-Never, 1-BusOff, 2-BusOff+RxErr, 3-All
+ * MSCAN_RIER_TSTATE - Transmitter Status Change Enable: 0-Never, 1-BusOff, 2-BusOff+TxErr, 3-All
  * MSCAN_RIER_OVRIE - Overrun
  * MSCAN_RIER_CSCIE - CAN Status Change
  */
-uint16_t mscan_IRQs = (MSCAN_TIER_TXE << 8) | MSCAN_RIER_RXFIE | MSCAN_RIER_OVRIE | MSCAN_RIER_CSCIE;  /* TX interrupt flag is held shifter */
+uint16_t mscan_IRQs = MSCAN_RIER_RXFIE | 
+               MSCAN_RIER_RSTATE | MSCAN_RIER_TSTATE |
+               MSCAN_RIER_OVRIE | MSCAN_RIER_CSCIE;    /* TX interrupt flag is held shifted */
 /* 1 - enable interrupt, 0 - interrupt is masked */
 
+/* used to distinguish which direction (TX/RX) caused Can Bus State change interrupt */
+reg_t rxstat = 0;
+reg_t txstat = 0;
+
+/* initial acceptance filter settings */
+#define INIT_STD_ID            0x0000
+#define INIT_STD_MASK          0x0fff  /* accept all messages - 11 mask bits and LSB is RTR */
+
+
 
 /* ************************************************************************************************************************************* */
 
@@ -94,19 +175,19 @@ int mscan_chip_config(struct canchip_t *chip)
 
        DEBUGMSG("Configuring chip...\n");
        
-       if (!(err = mscan_enable_configuration(chip)))
+       if ((err = mscan_enable_configuration(chip)))
                return err;
 
        if (!chip->baudrate)
                chip->baudrate=1000000;         /* default baudrate set to 1Mbit */
-       if (!(err = mscan_baud_rate(chip, chip->baudrate, chip->clock, 0, 750, 0)))
+       if ((err = mscan_baud_rate(chip, chip->baudrate, chip->clock, 0, 750, 0)))
                return err;
 
-       if (!(err = mscan_disable_configuration(chip)))
+       if ((err = mscan_disable_configuration(chip)))
                return err;
 
        /* IRQ mask could be set only when NOT in INIT mode */
-       if (!(err = mscan_config_irqs(chip, mscan_IRQs)))
+       if ((err = mscan_config_irqs(chip, mscan_IRQs)))
                return err;
 
        return 0;
@@ -116,47 +197,43 @@ int mscan_chip_config(struct canchip_t *chip)
 int mscan_enable_configuration(struct canchip_t *chip)
 {
        /* Do not clear buffers here, they may contain valid data ! */
-       int i = 0;
 
-       DEBUGMSG("Enabling configuration...\n");
+       DEBUGMSG("config ENABLE\n");
 
        if (mscan_init_mode_active(chip))       /* chip is already in config mode */
-               return 0;
+       {
+               DEBUGMSG("config ENABLE ... already in INIT mode\n");
+               goto econf_exit_ok;
+       }
 
        /* Disable interrupt */
        can_disable_irq(chip->chip_irq);
 
 
        /* Sleep mode - disable CAN activity after completing current operation */
-       mscan_set_flags(chip, MSCAN_CTL0_SLPRQ, MSCAN_CTL0);
-
-       /* Waits until chip enters Sleep mode - finishes current  TX/RX operation */
-       while (!mscan_get_flags(chip, MSCAN_CTL1_SLPAK, MSCAN_CTL1) && i++ < MSCAN_MAX_SETTING_WAIT_LOOPS)
-               udelay(200);
-
-       if (i >= MSCAN_MAX_SETTING_WAIT_LOOPS) {
-               CANMSG("Error entering Sleep mode (enable configuration) \n");
-               can_enable_irq(chip->chip_irq);
-               return -EBUSY;
-       }
-
+       /* if controler not synced to bus, skip sleep mode */
+       if (!mscan_sleep_mode_active(chip) && 
+               mscan_get_flags(chip, MSCAN_CTL0_SYNCH, MSCAN_CTL0))
+               if (mscan_enter_sleep_mode(chip))
+               {
+                       /* cannot enter sleep mode */
+                       DUMPREGS(chip);
+                       DEBUGMSG("Forcig INIT\n");
+               }
+               
 
        /* now we can enter Init mode */
-       mscan_set_flags(chip, MSCAN_CTL0_INITRQ, MSCAN_CTL0);
-
-       i = 0;  
-       /* Waits until chip enters Init mode */
-       while (!mscan_get_flags(chip, MSCAN_CTL1_INITAK, MSCAN_CTL1) && i++ < MSCAN_MAX_SETTING_WAIT_LOOPS)
-               udelay(200);
-
-       if (i >= MSCAN_MAX_SETTING_WAIT_LOOPS) {
-               CANMSG("Error entering Init mode (enable configuration) \n");
-               can_enable_irq(chip->chip_irq);
-               return -EBUSY;
-       }
-
+       if(mscan_enter_init_mode(chip))
+               goto econf_exit_busy;
 
+econf_exit_ok:
+       /* DEBUGMSG("config ENABLE ... OK\n"); */
        return 0;
+
+econf_exit_busy:
+       /* DEBUGMSG("config ENABLE ... failed !\n"); */
+       can_enable_irq(chip->chip_irq);
+       return -EBUSY;
 }
 
 int mscan_disable_configuration(struct canchip_t *chip)
@@ -164,10 +241,13 @@ int mscan_disable_configuration(struct canchip_t *chip)
        /* Do not clear buffers here, they may contain valid data ! */
        int i = 0;
 
-       DEBUGMSG("Disabling configuration mode...\n");
+       DEBUGMSG("config DISABLE\n");
 
        if (!mscan_init_mode_active(chip) && !mscan_sleep_mode_active(chip))    /* chip is already in normal mode */
+       {
+               DEBUGMSG("config DISABLE ... not in INIT mode\n");
                return 0;
+       }
 
 
        /* disable Init mode */
@@ -178,7 +258,7 @@ int mscan_disable_configuration(struct canchip_t *chip)
                udelay(200);
 
        if (i >= MSCAN_MAX_SETTING_WAIT_LOOPS) {
-               CANMSG("Error leaving Init mode (disable configuration) \n");
+               DEBUGMSG("Error leaving Init mode (disable configuration) \n");
                return -EBUSY;
        }
 
@@ -192,7 +272,7 @@ int mscan_disable_configuration(struct canchip_t *chip)
                udelay(200);
 
        if (i >= MSCAN_MAX_SETTING_WAIT_LOOPS) {
-               CANMSG("Error leaving Sleep mode (disable configuration) \n");
+               DEBUGMSG("Error leaving Sleep mode (disable configuration) \n");
                return -EBUSY;
        }
 
@@ -200,6 +280,8 @@ int mscan_disable_configuration(struct canchip_t *chip)
        /* Enable interrupt */
        can_enable_irq(chip->chip_irq);
 
+       /* DEBUGMSG("config DISABLE ... OK\n"); */
+
        return 0;
 }
 
@@ -228,8 +310,6 @@ int mscan_baud_rate(struct canchip_t *chip, int bitrate, int clock, int sjw, int
        reg_t btr0, btr1;
 
 
-       DEBUGMSG("Seting Baud rate...\n");
-
         /* Use CIA recommended sample points */
         if (!sampl_pt)
         {
@@ -291,10 +371,10 @@ int mscan_baud_rate(struct canchip_t *chip, int bitrate, int clock, int sjw, int
 
        DEBUGMSG("Setting %lu bps.\n", best_rate);
 
-       /* all values has offset -1 */
-       brp--; tseg1--; tseg2--;
+       /* all values has offset -1 in MSCAN memory */
+       best_brp--; tseg1--; tseg2--;
 
-       btr0 = (brp & MSCAN_BTR0_BRP) | ((sjw << 6) & MSCAN_BTR0_SJW);
+       btr0 = (best_brp & MSCAN_BTR0_BRP) | ((sjw << 6) & MSCAN_BTR0_SJW);
        btr1 = (tseg1 & MSCAN_BTR1_TSEG1) | ((tseg2 << 4) & MSCAN_BTR1_TSEG2);
 
        mscan_set_btregs(chip, btr0, btr1);     
@@ -315,6 +395,8 @@ int mscan_set_btregs(struct canchip_t *chip, unsigned short bcr0, unsigned short
 
        btr1 &= ~MSCAN_BTR1_SAMP;       /* use one-point sample, not three smaples per bit */
 
+       DEBUGMSG("BTR0 0x%02x  BTR1 0x%02x\n", btr0, btr1);
+       
        can_write_reg(chip, btr0, MSCAN_BTR0);
        can_write_reg(chip, btr1, MSCAN_BTR1);
        
@@ -357,10 +439,12 @@ int mscan_attach_to_chip(struct canchip_t *chip)
 /*     DEBUGMSG("Attaching to chip %d.\n", chip->chip_idx); */
        reg_t ctl1;
 
+
        /* Clear all TX and RX buffers */
        if (mscan_clear_objects(chip))
                return -ENODEV;
 
+
        /* Transmitt Abort Request Register (TARQ) is clean after INIT mode - no need to clear it explicitly */
        /* Control Register 0 (CTL0) is clean after INIT too (excepts fro WUPE, SLRQ and INITRQ) */
        
@@ -376,9 +460,16 @@ int mscan_attach_to_chip(struct canchip_t *chip)
                /* MSCAN_CTL1_WUPM |            WakeUp mode not used */
                /* MSCAN_CTL1_SLPAK |           ReadOnly */
                /* MSCAN_CTL1_INITAK |          ReadOnly */
-
+       
        can_write_reg(chip, ctl1, MSCAN_CTL1);
 
+
+       /* set filter to accept all STD messages */
+       if (mscan_standard_mask(chip, INIT_STD_ID, INIT_STD_MASK))
+       {
+               DEBUGMSG("Failed to set initial filter ID: %d Mask: 0x%04x\n", INIT_STD_ID, INIT_STD_MASK);
+       }
+
        /* set Baudrate and Interrupts */
        if (mscan_chip_config(chip))
                return -ENODEV;
@@ -387,19 +478,23 @@ int mscan_attach_to_chip(struct canchip_t *chip)
        if (mscan_disable_configuration(chip))
                return -ENODEV;
 
-       /* Enable interrupt */
-       can_enable_irq(chip->chip_irq);
+       /* Enable interrupt - called in mpc5200_request_io */
+       /* can_enable_irq(chip->chip_irq); */
+
+       DEBUGMSG("Successfully attached to chip [%02d].\n", chip->chip_idx);
 
-       CANMSG("Successfully attached to chip [%02d].\n", chip->chip_idx);
        return 0;
 }
 
 int mscan_release_chip(struct canchip_t *chip)
 {
-       mscan_stop_chip(chip);          /* we are in INIT mode */
+       /* IRQ unmapped in lincan driver core */
        can_disable_irq(chip->chip_irq);
        
-       mscan_clear_objects(chip);
+       mscan_clear_objects(chip);      /* Cannot be called in INIT mode */
+
+
+       mscan_stop_chip(chip);          /* we are in INIT mode */
 
        /* disable chip */
        mscan_clear_flags(chip, MSCAN_CTL1_CANE, MSCAN_CTL1);
@@ -424,13 +519,13 @@ int mscan_standard_mask(struct canchip_t *chip, unsigned short code, unsigned sh
 
 
        /* we use two 32-bit acceptance filters */
-       mscan_set_flags(chip, 0x30, MSCAN_IDAC);
+       mscan_clear_flags(chip, MSCAN_IDAC_IDAM, MSCAN_IDAC);
 
 
        idr0 = (reg_t)((code & 0x0ff0) >> 4);   /* 8 most significant bits */
-       idr1 = (reg_t)((code & 0x000f) << 4);   /* 3 last bits, RTR as MSB, IDE=0 doesnt have to be set explicitly */
+       idr1 = (reg_t)((code & 0x000f) << 4);   /* 3 last bits, RTR as LSB, IDE=0 doesnt have to be set explicitly */
        mr0 =  (reg_t)((mask & 0x0ff0) >> 4);
-       mr1 =  (reg_t)((mask & 0x000f) << 4);
+       mr1 =  (reg_t)(((mask & 0x000f) << 4) & 0xf0);  /* to be sure IDE=0 */
 
 
        can_write_reg(chip, idr0, MSCAN_IDAR0);
@@ -439,7 +534,17 @@ int mscan_standard_mask(struct canchip_t *chip, unsigned short code, unsigned sh
        can_write_reg(chip, mr0, MSCAN_IDMR0);
        can_write_reg(chip, mr1, MSCAN_IDMR1);
 
+       /* use same filtering settings for second filter */
+       can_write_reg(chip, idr0, MSCAN_IDAR4);
+       can_write_reg(chip, idr1, MSCAN_IDAR5);
+
+       can_write_reg(chip, mr0, MSCAN_IDMR4);
+       can_write_reg(chip, mr1, MSCAN_IDMR5);
+
+
        DEBUGMSG("Set standard_mask [id:0x%04x RTR=%d, m:0x%04x RTR=%d]\n", code >> 1, code & 0x0001, mask >> 1, mask & 0x0001);
+       DUMPFLT(chip);
+
        return 0;
 }
 
@@ -454,7 +559,7 @@ int mscan_extended_mask(struct canchip_t *chip, unsigned long code, unsigned lon
 
 
        /* we use two 32-bit acceptance filters */
-       mscan_set_flags(chip, 0x30, MSCAN_IDAC);
+       mscan_clear_flags(chip, MSCAN_IDAC_IDAM, MSCAN_IDAC);
 
        
        idr0 = (reg_t)((code & 0x7fa00000) >> 22);      /* EXT_ID {29:21} */
@@ -487,7 +592,20 @@ int mscan_extended_mask(struct canchip_t *chip, unsigned long code, unsigned lon
        can_write_reg(chip, mr2, MSCAN_IDMR2);
        can_write_reg(chip, mr3, MSCAN_IDMR3);
 
+       /* use same filtering settings for second filter */
+       can_write_reg(chip, idr0, MSCAN_IDAR4);
+       can_write_reg(chip, idr1, MSCAN_IDAR5);
+       can_write_reg(chip, idr2, MSCAN_IDAR6);
+       can_write_reg(chip, idr3, MSCAN_IDAR7);
+
+       can_write_reg(chip, mr0, MSCAN_IDMR4);
+       can_write_reg(chip, mr1, MSCAN_IDMR5);
+       can_write_reg(chip, mr2, MSCAN_IDMR6);
+       can_write_reg(chip, mr3, MSCAN_IDMR7);
+
+
        DEBUGMSG("Set extended_mask [id:0x%08x RTR=%lu, m:0x%08x RTR=%lu]\n", (uint32_t)(code >> 1), code & 0x00000001, (uint32_t)(mask >> 1), mask & 0x00000001);
+       DUMPFLT(chip);
     
         return 0;
 }
@@ -498,7 +616,7 @@ int mscan_extended_mask(struct canchip_t *chip, unsigned long code, unsigned lon
 
 int mscan_pre_read_config(struct canchip_t *chip, struct msgobj_t *obj)
 {
-       DEBUGMSG("Pre read config\n");
+       DEBUGRX("Pre read config\n");
 
        /* MSCAN has only one buffer, which is already initialized */
        
@@ -508,41 +626,64 @@ int mscan_pre_read_config(struct canchip_t *chip, struct msgobj_t *obj)
 int mscan_pre_write_config(struct canchip_t *chip, struct msgobj_t *obj, struct canmsg_t *msg)
 {
        reg_t txf;
+       reg_t bf;
        int buff_no;
 
-       DEBUGMSG("Pre write config\n");
-
+       can_spin_lock(&mscan_prewr_lock);
        can_preempt_disable();
+       DEBUGTX("Pre write config\n");
        
        /* find free buffer */
        txf = mscan_get_flags(chip, MSCAN_TFLG_TXE, MSCAN_TFLG);
-       for (buff_no = 0; buff_no < 3 && (txf ^ (0x01 << buff_no)); buff_no++) { }
+       for (buff_no = 0; buff_no < 3 && !(txf & (0x01 << buff_no)); buff_no++) { }
 
        if (buff_no >= 3)
-               return -ENODEV;         /* no free buffer found */
+       {
+               DEBUGTX("no free buffer found [0x%02x]\n", txf);
+               goto pwrite_exit_fail;          /* no free buffer found */
+       }
 
-       mscan_setup_txbuffer(chip, msg, buff_no);
 
-       /* clear TX Buffer Empty flag (by writing '1') to initiate trasmition */
-       mscan_set_flags(chip, (reg_t)(0x01 << buff_no), MSCAN_TFLG);
+       /* Select buffer */
+       bf = (reg_t)(0x01 << buff_no);
+       can_write_reg(chip, bf & MSCAN_TBSEL_TX, MSCAN_TBSEL);
 
-       can_preempt_enable();
+       DEBUGTX("Buffer %d\n", buff_no);
+
+       mscan_setup_txbuffer(chip, msg);
 
+       /* store buffer flag into canmsg_t->cob for future
+        * use in send_msg
+        * canmsg_t->cob is not used according to Lincan documentation */
+       msg->cob = bf;
+
+
+       can_preempt_enable();
+       can_spin_unlock(&mscan_prewr_lock);
        return 0;
+
+pwrite_exit_fail:
+       can_preempt_enable();
+       can_spin_unlock(&mscan_prewr_lock);
+       return -ENODEV;         /* no free buffer found */
 }
 
 int mscan_send_msg(struct canchip_t *chip, struct msgobj_t *obj, struct canmsg_t *msg)
 {
-       DEBUGMSG("Send Message\n");
+       DEBUGTX("Send Message\n");
        
-       /* nothing to do here - message is already set to transmit in pre_write_config */
+       /* turn on IRQ for this buffer */
+       can_write_reg(chip, msg->cob, MSCAN_TIER);
+
+       /* clear TX Buffer Empty flag (by writing '1') to initiate trasmition */
+       can_write_reg(chip, msg->cob, MSCAN_TFLG);
        
        return 0;
 }
 
 int mscan_remote_request(struct canchip_t *chip, struct msgobj_t *obj)
 {
-       CANMSG("mscan_remote_request not implemented\n");
+       DEBUGMSG("mscan_remote_request not implemented\n");
        return -ENOSYS;
 }
 
@@ -553,35 +694,35 @@ int mscan_remote_request(struct canchip_t *chip, struct msgobj_t *obj)
 int mscan_irq_handler(int irq, struct canchip_t *chip)
 {
 /*
-       MSCAN_RFLG_WUPIF -  WakeUp Interrupt Flag - rw
-        MSCAN_RFLG_CSCIF -  CAN Status Change Interrupt Flag - rw
-        MSCAN_RFLG_RSTAT -  Receiver Status Bits: 0-RxOK, 1-RxWRN, 2-RxERR, 3-BusOff - ro
-        MSCAN_RFLG_TSTAT -  Transmitter Status Bits: 0-TxOK, 1-TxWRN, 2-TxErr, 3-BusOff - ro
-        MSCAN_RFLG_OVRIF -  Overrun Interrupt Flag - rw
-        MSCAN_RFLG_RXF   -  Receive Buffer Full - rw
+        MSCAN_RFLG_CSCIF -  CAN Status Change Interrupt Flag
+        MSCAN_RFLG_RSTAT -  Receiver Status Bits: 0-RxOK, 1-RxWRN, 2-RxERR, 3-BusOff
+        MSCAN_RFLG_TSTAT -  Transmitter Status Bits: 0-TxOK, 1-TxWRN, 2-TxErr, 3-BusOff
+        MSCAN_RFLG_OVRIF -  Overrun Interrupt Flag
+        MSCAN_RFLG_RXF   -  Receive Buffer Full
 
         MSCAN_TFLG_TXE   - Transmitter Buffer Empty */
 
        /* get IRQs */
-       uint16_t irq_reg;
+       uint16_t irq_reg, tx_irq;
+       reg_t irq_rstat, irq_tstat;
        short loop_cnt = MSCAN_MAX_IRQ_WAIT_LOOPS;
 
-       irq_reg =       (mscan_get_flags(chip, (reg_t)((mscan_IRQs >> 8) & 0xff), MSCAN_TFLG) << 8) |
-                       (mscan_get_flags(chip, (reg_t)(mscan_IRQs & 0xff), MSCAN_RFLG));
-       DEBUGMSG("irq: %d", irq);
-       DEBUGMSG("IRQ Handler: MSCAN_IRR: 0x%04x\n", irq_reg); 
+       tx_irq = (MSCAN_TFLG_TXE << 8);         /* to spare few tacts in IRQ loop */
+
+       irq_reg = mscan_get_irq_flags(chip);
+       DEBUGMSG("IRQ Handler chip %d (%d)\n", chip->chip_idx, irq); 
+
 
        do {
+               if(!loop_cnt--) 
+                       goto irqhnd_exit_stuck;
 
-               if(!loop_cnt--) {
-                       CANMSG("mscan_irq_handler IRQ %d stuck\n", irq);
-                       return CANCHIP_IRQ_STUCK;
-               }
+               DEBUGMSG("IRR: 0x%04x\n", irq_reg);
 
                /* Received message */
                if (irq_reg & MSCAN_RFLG_RXF)
                {
-                       DEBUGMSG("Received message");
+                       DEBUGRX("Received message\n");
 
                        mscan_irq_read_handler(chip, chip->msgobj[0]);
                        /* RXPR flag for this msgobj is cleared during irq_read_handler*/
@@ -590,61 +731,79 @@ int mscan_irq_handler(int irq, struct canchip_t *chip)
                        mscan_set_flags(chip, MSCAN_RFLG_RXF, MSCAN_RFLG);
                }
 
-               /* Receive bus off/error/warning */
-               if (irq_reg & MSCAN_RFLG_RSTAT)
+               
+               /* Can Status change - due to RX/TX error counters */
+               if(irq_reg & MSCAN_RFLG_CSCIF)
                {
-                       switch (((irq_reg & MSCAN_RFLG_RSTAT) >> 4) & 0x03)
-                       {
-                               case 3: /* Bus off */
-                                       CANMSG("Error: entering RX BUS OFF state");
+                       irq_rstat = (irq_reg >> 4) & 0x03;
+                       irq_tstat = (irq_reg >> 2) & 0x03;
 
-                                       chip->msgobj[0]->ret=-1;
 
-                                       /* notify RX */
-                                       mscan_notifyRXends(chip->msgobj[0], CANQUEUE_NOTIFY_ERROR);
-                                       break;
-
-                               case 2: /* bus - error passive */
-                                       CANMSG("Warning: entering ERROR PASSIVE state REC: %d\n", (reg_t)can_read_reg(chip, MSCAN_RXERR));
-                                       /* Show warning only */
-                                       break;
-                       
-                               case 1: /* bus - warning */
-                                       CANMSG("Bus Warning: REC: %d\n", (reg_t)can_read_reg(chip, MSCAN_RXERR));
-                                       /* Show warning only */
-                                       break;
+                       /* Receive bus off/error/warning */
+                       if (irq_rstat ^ rxstat) /* check whether RSTAT changed */
+                       {
+                               switch (irq_rstat)
+                               {
+                                       case 3: /* Bus off */
+                                               MSCAN_STAT_CHANGE("RX: BUS OFF", chip->chip_idx);
+
+                                               chip->msgobj[0]->ret=-1;
+
+                                               /* notify RX */
+                                               mscan_notifyRXends(chip->msgobj[0], CANQUEUE_NOTIFY_ERROR);
+                                               break;
+
+                                       case 2: /* bus - error passive */
+                                               MSCAN_STAT_CHANGE("RX: ERROR PASSIVE", chip->chip_idx);
+                                               /* Show warning only */
+                                               break;
+                               
+                                       case 1: /* bus - warning */
+                                               MSCAN_STAT_CHANGE("RX: Bus Warning", chip->chip_idx);
+                                               /* Show warning only */
+                                               break;
+
+                                       case 0: /* bus - OK */
+                                               MSCAN_STAT_CHANGE("RX: Bus OK", chip->chip_idx);
+                                               /* Show warning only */
+                                               break;
+                               }
+                               rxstat = irq_rstat;     /* update static RX status field */
                        }
-                       
-                       /* reset flag */
-                       mscan_set_flags(chip, MSCAN_RFLG_RSTAT, MSCAN_RFLG);
-               }
-
-               /* Transmit bus off/error/warning */
-               if (irq_reg & MSCAN_RFLG_TSTAT)
-               {
-                       switch (((irq_reg & MSCAN_RFLG_TSTAT) >> 1) & 0x03)
+       
+                       /* Transmit bus off/error/warning */
+                       if (irq_tstat ^ txstat)
                        {
-                               case 3: /* Bus off */
-                                       CANMSG("Error: entering TX BUS OFF state");
-
-                                       /* notify TX end */
-                                       if(chip->msgobj[0]->tx_slot)
-                                               canque_notify_inends(chip->msgobj[0]->tx_qedge, CANQUEUE_NOTIFY_ERROR);
-                                       break;
-
-                               case 2: /* bus - error passive */
-                                       CANMSG("Warning: entering ERROR PASSIVE state TEC: %d\n", (reg_t)can_read_reg(chip, MSCAN_TXERR));
-                                       /* Show warning only */
-                                       break;
-                       
-                               case 1: /* bus - warning */
-                                       CANMSG("Bus Warning: TEC: %d\n", (reg_t)can_read_reg(chip, MSCAN_TXERR));
-                                       /* Show warning only */
-                                       break;
+                               switch (irq_tstat)
+                               {
+                                       case 3: /* Bus off */
+                                               MSCAN_STAT_CHANGE("TX: BUS OFF", chip->chip_idx);
+
+                                               /* notify TX end */
+                                               if(chip->msgobj[0]->tx_slot)
+                                                       canque_notify_inends(chip->msgobj[0]->tx_qedge, CANQUEUE_NOTIFY_ERROR);
+                                               break;
+
+                                       case 2: /* bus - error passive */
+                                               MSCAN_STAT_CHANGE("TX: ERROR PASSIVE", chip->chip_idx);
+                                               /* Show warning only */
+                                               break;
+                               
+                                       case 1: /* bus - warning */
+                                               MSCAN_STAT_CHANGE("TX: Bus Warning", chip->chip_idx);
+                                               /* Show warning only */
+                                               break;
+
+                                       case 0: /* bus - OK */
+                                               MSCAN_STAT_CHANGE("TX: Bus OK", chip->chip_idx);
+                                               /* Show warning only */
+                                               break;
+                               }
+                               txstat = irq_tstat;     /* update static TX status field */
                        }
-                       
+
                        /* reset flag */
-                       mscan_set_flags(chip, MSCAN_RFLG_TSTAT, MSCAN_RFLG);
+                       mscan_set_flags(chip, MSCAN_RFLG_CSCIF, MSCAN_RFLG);
                }
 
                /* Message Overrun/Overwritten */
@@ -660,49 +819,64 @@ int mscan_irq_handler(int irq, struct canchip_t *chip)
                }
 
                /* Mailbox empty - after message was sent */
-               if ((irq_reg >> 8) & MSCAN_TFLG_TXE)
+               if (irq_reg & tx_irq)
                {
-                   /* Clear TXACK flag */                  
-                   mscan_set_flags(chip, MSCAN_TFLG_TXE, MSCAN_TFLG);
+                       DEBUGTX("Buffer empty interrupt\n");
+
+                       /* clear propriate TXE IRE Enabled  flag */
+                       can_spin_lock(mscan_prewr_lock);
+                       mscan_clear_flags(chip, (irq_reg >> 8) & MSCAN_TIER_TXE, MSCAN_TIER);
+                       can_spin_unlock(mscan_prewr_lock);
+
 
-                   /* sends message */     
-                   mscan_wakeup_tx(chip, chip->msgobj[0]);
+                       /* sends message */         
+                       mscan_wakeup_tx(chip, chip->msgobj[0]);
                }
 
-               irq_reg =       (mscan_get_flags(chip, (reg_t)((mscan_IRQs >> 8) & 0xff), MSCAN_TFLG) << 8) |
-                               (mscan_get_flags(chip, (reg_t)(mscan_IRQs & 0xff), MSCAN_RFLG));
-       } while(irq_reg & mscan_IRQs);
+               irq_reg = mscan_get_irq_flags(chip);
+
+       /* omit RSTAT and TSTAT - they're used only in CSCIF handler */
+       } while(irq_reg &
+               mscan_IRQs & 
+               ~(MSCAN_RFLG_RSTAT | MSCAN_RFLG_TSTAT));
+
 
        return CANCHIP_IRQ_HANDLED;
+
+irqhnd_exit_stuck:
+       DEBUGMSG("mscan_irq_handler IRQ %d stuck\n", irq);
+       return CANCHIP_IRQ_STUCK;
 }
 
 int mscan_irq_accept(int irq, struct canchip_t *chip)
 {
-       CANMSG("mscan_irq_accept NOT IMPLEMENTED\n");
+       DEBUGMSG("mscan_irq_accept NOT IMPLEMENTED\n");
        return -ENOSYS;
 }
 
 int mscan_config_irqs(struct canchip_t *chip, short irqs)
 {
        int err;
-       reg_t tier, rier;
+       reg_t rier, tier;
+
+       DEBUGMSG("config IRQs\n");
 
        if (mscan_init_mode_active(chip))
        {
-               CANMSG("MSCAN: Setting Up IRQs while INIT active \n");
+               DEBUGMSG("MSCAN: Setting Up IRQs while INIT active \n");
                return -EBUSY;
        }
 
-       tier = (reg_t)((irqs >> 8) & 0x00ff & ~MSCAN_TIER_RSVD);
        rier = (reg_t)(irqs & 0x00ff);
+       tier = (reg_t)((irqs >> 8) & 0x00ff);
 
-       if(!(err = mscan_clear_irq_flags(chip)))
+       if((err = mscan_clear_irq_flags(chip)))
                return err;
 
        can_write_reg(chip, rier, MSCAN_RIER);
        can_write_reg(chip, tier, MSCAN_TIER);
 
-       CANMSG("IRQ Mask set [0x%02x]\n", irqs);
+       DEBUGMSG("IRQ Mask set [rier: 0x%02x]\n", rier);
        return 0;
 }
 
@@ -742,35 +916,39 @@ int mscan_check_tx_stat(struct canchip_t *chip)
 int mscan_check_txbuff_stat(struct canchip_t *chip, int buffer)
 {
        /* Transmition is complete return 0 - no error */
-       reg_t txf = mscan_get_flags(chip, MSCAN_TFLG_TXE, MSCAN_TFLG);
+       reg_t txf = mscan_get_flags(chip, MSCAN_TFLG_TXE & (0x01 << buffer), MSCAN_TFLG);
        return (txf ^ (0x01 << buffer));
 }
 
 int mscan_wakeup_tx(struct canchip_t *chip, struct msgobj_t *obj)
 {
-       DEBUGMSG("WakeUP TX\n");
-
        can_preempt_disable();
+       DEBUGMSG("WakeUP TX\n");
        
-       can_msgobj_set_fl(obj,TX_REQUEST);
-       if(!can_msgobj_test_and_set_fl(obj,TX_LOCK) &&
-               (!mscan_check_txbuff_stat(chip, 0) ||
-                !mscan_check_txbuff_stat(chip, 1) ||
-                !mscan_check_txbuff_stat(chip, 2)))
-       {       /* enable transmition only if MB is empty */
-               can_msgobj_clear_fl(obj,TX_REQUEST);
-
-               mscan_irq_write_handler(chip, obj);
-       
-               can_msgobj_clear_fl(obj,TX_LOCK);
-       }
-       else
-               can_msgobj_clear_fl(obj,TX_REQUEST);
+        can_msgobj_set_fl(obj, TX_REQUEST);
+        while(!can_msgobj_test_and_set_fl(obj, TX_LOCK)) {
+                can_msgobj_clear_fl(obj, TX_REQUEST);
+
+               /* enable transmition only if any TX buffer is empty */
+                if (mscan_get_flags(chip, MSCAN_TFLG_TXE, MSCAN_TFLG))
+                        mscan_irq_write_handler(chip, obj);
+               else
+               {
+                       can_msgobj_clear_fl(obj, TX_LOCK);
+                       break;
+               }
+
+
+                can_msgobj_clear_fl(obj, TX_LOCK);
+
+                if(!can_msgobj_test_fl(obj, TX_REQUEST))
+                       break;
+        }
 
 
        can_preempt_enable();
 
-/*     DEBUGMSG("WakeUP TX - END\n"); */
+       DEBUGMSG("WakeUP TX - END\n");
        return 0;
 }
 
@@ -780,7 +958,7 @@ int mscan_filtch_rq(struct canchip_t *chip, struct msgobj_t * obj)
        uint32_t mask;
 
 
-#if myDEBUG
+#if MPC5200_DBG
        int num = canqueue_ends_filt_conjuction(obj->qends, &filter);
 #else
        canqueue_ends_filt_conjuction(obj->qends, &filter);
@@ -811,6 +989,7 @@ void mscan_irq_read_handler(struct canchip_t *chip, struct msgobj_t *obj)
        if (!mscan_get_flags(chip, MSCAN_RFLG_RXF, MSCAN_RFLG))
        {
                obj->ret=-1;
+               DEBUGRX("Receive buffer empty\n");
                return;
        }
 
@@ -826,6 +1005,8 @@ void mscan_irq_write_handler(struct canchip_t *chip, struct msgobj_t *obj)
 {
        int cmd;
 
+       DEBUGTX("Irq write handler\n");
+
        if(obj->tx_slot){
                /* Do local transmitted message distribution if enabled */
                if (processlocal){
@@ -934,6 +1115,8 @@ int mscan_fill_chipspecops(struct canchip_t *chip)
 
 int mscan_reset_chip(struct canchip_t * chip)
 {
+       DEBUGMSG("chip reset \n");
+
        /* reset chip by turning MSCAN off and on in INIT mode */
        if (mscan_enable_configuration(chip))
                return -ENODEV;
@@ -944,17 +1127,23 @@ int mscan_reset_chip(struct canchip_t * chip)
        if (mscan_disable_configuration(chip))
                return -ENODEV;
 
+       /* Abort all pending messages */
+       mscan_abort_msg(chip, MSCAN_TARQ_ABTRQ);
+
+       DEBUGMSG("chip reset DONE\n");
+
        return 0;
 }
 
 /* mscan_clear_irq_flags should be called only when not in INIT mode */
 int mscan_clear_irq_flags(struct canchip_t *chip)
 {
-       reg_t rflg, tflg;
+       /* do not clear TXE flags -> it forces chip to send messages in buffers */
+       reg_t rflg;
 
        if (mscan_init_mode_active(chip))
        {
-               CANMSG("MSCAN: Clearing IRQs while INIT active \n");
+               DEBUGMSG("MSCAN: Clearing IRQs while INIT active \n");
                return -EBUSY;
        }
 
@@ -962,11 +1151,9 @@ int mscan_clear_irq_flags(struct canchip_t *chip)
 
        rflg =  MSCAN_RFLG_WUPIF | MSCAN_RFLG_CSCIF | MSCAN_RFLG_RSTAT |
                MSCAN_RFLG_TSTAT | MSCAN_RFLG_OVRIF | MSCAN_RFLG_RXF;
-       tflg =  MSCAN_TFLG_TXE;
 
        /* reset flags by writing '1' */
        can_write_reg(chip, rflg, MSCAN_RFLG);
-       can_write_reg(chip, tflg, MSCAN_TFLG);
 
        return 0;
 }
@@ -985,14 +1172,11 @@ void mscan_notifyRXends(struct msgobj_t * obj, int what)
 
 /* Fill message content to one transmit buffers */
 /* Note: Check whether buffer is empy has to be done before calling this function */
-void mscan_setup_txbuffer(struct canchip_t * chip, struct canmsg_t * msg, int buffer)
+void mscan_setup_txbuffer(struct canchip_t * chip, struct canmsg_t * msg)
 {
-       volatile struct mscan_msg_buffer * txb = (struct mscan_msg_buffer *)MSCAN_TXFG;
+       volatile struct mscan_msg_buffer * txb = (struct mscan_msg_buffer *)(chip->chip_base_addr + MSCAN_TXFG);
        reg_t rtr;
 
-       /* Select buffer */
-       mscan_set_flags(chip, (0x01 << buffer) & MSCAN_TBSEL_TX, MSCAN_TBSEL);
-
 
        if (msg->flags & MSG_RTR)
                rtr = 0x01;
@@ -1041,12 +1225,14 @@ void mscan_setup_txbuffer(struct canchip_t * chip, struct canmsg_t * msg, int bu
 
        /* data length */
        txb->data_len = (reg_t)(msg->length & 0x1f);
+
+       DUMPBUFF(chip, MSCAN_TXFG);
 }
 
 /* Fill message content from receive buffer */
 void mscan_msg_from_rxbuffer(struct canchip_t * chip, struct canmsg_t * msg)
 {
-       volatile struct mscan_msg_buffer * rxb = (struct mscan_msg_buffer *)MSCAN_RXFG;
+       struct mscan_msg_buffer * rxb = (struct mscan_msg_buffer *)(chip->chip_base_addr + MSCAN_RXFG);
 
        /* retrieve Can mesage ID */
        msg->flags = 0; /* clear all */
@@ -1054,6 +1240,7 @@ void mscan_msg_from_rxbuffer(struct canchip_t * chip, struct canmsg_t * msg)
        /* check buffer IDE flag */
        if (mscan_get_flags(chip, (reg_t)0x08, MSCAN_RXFG + MSCAN_MSGBUFF_ID1)) /* 0x08 - IDE flag in ID1 */
        {
+               DEBUGMSG("Ext ID\n");
                /* Extended ID */
                msg->id =       (((uint32_t)rxb->id_0 << 21) & 0x3fa00000) |
                                (((uint32_t)rxb->id_1 << 18) & 0x00380000) |
@@ -1070,8 +1257,9 @@ void mscan_msg_from_rxbuffer(struct canchip_t * chip, struct canmsg_t * msg)
        }
        else
        {
+               DEBUGMSG("std ID\n");
                /* Standard ID */
-               msg->id =       (((uint32_t)rxb->id_0 << 3) & 0x000001f8) |
+               msg->id =       (((uint32_t)rxb->id_0 << 3) & 0x000007f8) |
                                (((uint32_t)rxb->id_1 >> 5) & 0x00000007);
 
                /* RTR flag */
@@ -1100,6 +1288,11 @@ void mscan_msg_from_rxbuffer(struct canchip_t * chip, struct canmsg_t * msg)
 /* updates the TSEG1 and TSEG2 according to overall TSEG and Sample Point (0-1000) */
 static int mscan_update_samplept(int sampl_pt, int tseg, int *tseg1, int *tseg2)
 {
+       /* tseg = tseg1 + tseg2, its NOT tsegall !
+        * difference between tseg and tsegall is:
+        *      tsegall = tseg + 1
+        */
+       
         *tseg2 = tseg + 1 - (sampl_pt * (tseg + 1)) / 1000;
         if (*tseg2 < MSCAN_TSEG2_MIN)
                 *tseg2 = MSCAN_TSEG2_MIN;
@@ -1110,14 +1303,14 @@ static int mscan_update_samplept(int sampl_pt, int tseg, int *tseg1, int *tseg2)
                 *tseg1 = MSCAN_TSEG1_MAX;
                 *tseg2 = tseg - *tseg1;
         }
-        return 1000 * (tseg + 1 - *tseg2) / (tseg + 1);
+        return 1000 * (*tseg1 + 1) / (tseg + 1);
 }
 
 
 static int mscan_init_mode_active(struct canchip_t *chip)
 {
        /* Init request AND Init ACK */
-       DEBUGMSG(" is Init active?\n");
+       /* DEBUGMSG(" is Init active?\n"); */
        return  mscan_get_flags(chip, MSCAN_CTL0_INITRQ, MSCAN_CTL0) &&
                mscan_get_flags(chip, MSCAN_CTL1_INITAK, MSCAN_CTL1);
 }
@@ -1128,10 +1321,56 @@ static int mscan_sleep_mode_active(struct canchip_t *chip)
                mscan_get_flags(chip, MSCAN_CTL1_SLPAK, MSCAN_CTL1);
 }
 
+static int mscan_enter_init_mode(struct canchip_t * chip)
+{
+       int i = MSCAN_MAX_SETTING_WAIT_LOOPS + 1;
+
+       mscan_set_flags(chip, MSCAN_CTL0_INITRQ, MSCAN_CTL0);
+
+       /* Waits until chip enters Init mode */
+       while (!mscan_get_flags(chip, MSCAN_CTL1_INITAK, MSCAN_CTL1) && --i)
+               udelay(200);
+
+       if (!i)
+       {
+               DEBUGMSG("Error entering Init mode (enable configuration) \n");
+               return -ENODEV;
+       }
+
+       return 0;
+}
+static int mscan_enter_sleep_mode(struct canchip_t * chip)
+{
+       int i = MSCAN_MAX_SETTING_WAIT_LOOPS + 1;
+
+       if (mscan_sleep_mode_active(chip))
+       {
+               DEBUGMSG("Already in sleep mode or not synced to Bus\n");
+               return 0;
+       }
+
+       /* DEBUGMSG("isSynced: 0x%02x\n",mscan_get_flags(chip, MSCAN_CTL0_SYNCH, MSCAN_CTL0)); */
+
+       mscan_set_flags(chip, MSCAN_CTL0_SLPRQ, MSCAN_CTL0);
+
+       /* Waits until chip enters Sleep mode - finishes current  TX/RX operation */
+       while (!mscan_get_flags(chip, MSCAN_CTL1_SLPAK, MSCAN_CTL1) && --i)
+               udelay(200);
+
+       if (!i)
+       {
+               DEBUGMSG("Error entering Sleep mode (enable configuration) \n");
+               return -ENODEV;
+       }
+
+       return 0;
+}
+
+
 
 static reg_t mscan_get_flags(struct canchip_t * chip, reg_t flags, unsigned reg_addr)
 {
-       DEBUGMSG("  get flags [%u]\n", reg_addr);
+       /* DEBUGMSG("  get flags [%u]\n", reg_addr); */
        return (reg_t)can_read_reg(chip, reg_addr) & flags;
 }
 static void mscan_set_flags(struct canchip_t * chip, reg_t flags, unsigned reg_addr)
@@ -1144,6 +1383,13 @@ static void mscan_clear_flags(struct canchip_t * chip, reg_t flags, unsigned reg
        reg_t r = can_read_reg(chip, reg_addr);
        can_write_reg(chip, r & ~flags, reg_addr);
 }
+static uint16_t mscan_get_irq_flags(struct canchip_t * chip)
+{
+       /* Transmit Buffer Empty only if enabled */
+       return ((mscan_get_flags(chip, MSCAN_TIER_TXE, MSCAN_TIER) & 
+                mscan_get_flags(chip, MSCAN_TFLG_TXE, MSCAN_TFLG)) << 8) |
+               (mscan_get_flags(chip, (reg_t)(mscan_IRQs & 0xff), MSCAN_RFLG));
+}
 
 
 static void mscan_clear_buffer(struct canchip_t * chip, unsigned start_addr)
@@ -1153,3 +1399,44 @@ static void mscan_clear_buffer(struct canchip_t * chip, unsigned start_addr)
        for (addr = start_addr; addr < 26; addr++)
                can_write_reg(chip, 0x00, addr);
 }
+
+static int mscan_abort_msg(struct canchip_t * chip, reg_t buffer_mask)
+{
+       reg_t m;
+       int i = MSCAN_MAX_SETTING_WAIT_LOOPS + 1;
+
+       /* check input */
+       m = buffer_mask & MSCAN_TARQ_ABTRQ;
+
+       /* It's not neccessary to abort empy message */
+       m &= (~mscan_get_flags(chip, MSCAN_TFLG_TXE, MSCAN_TFLG));
+
+       if(!m)
+       {
+               DEBUGMSG("nothing to abort\n");
+               goto abortmsg_exit_ok;
+       }
+
+       
+       /* set TARQ - Transmitt Abort Request */
+       mscan_set_flags(chip, m, MSCAN_TARQ);
+
+
+       /* Waits until chip acknowleges abortition */
+       while (mscan_get_flags(chip, MSCAN_TAAK_ABTAK, MSCAN_TAAK) != m  && --i)
+               udelay(200);
+
+       if (!i)
+       {
+               DEBUGMSG("Error waiting for Message Abort [requested: 0x%02x]\n", m);
+               goto abortmsg_exit_fail;
+       }
+
+
+abortmsg_exit_ok:
+       return 0;
+
+abortmsg_exit_fail:
+       return -ENODEV;
+}
+