Diff: targets/hal/TARGET_NXP/TARGET_LPC176X/can_api.c

Revision:

637:ed69428d4850

Parent:

530:2939f5396008

--- a/targets/hal/TARGET_NXP/TARGET_LPC176X/can_api.c	Thu Nov 26 11:20:41 2015 +0000
+++ b/targets/hal/TARGET_NXP/TARGET_LPC176X/can_api.c	Tue Dec 22 13:22:16 2015 +0000
@@ -63,6 +63,9 @@
 };
 typedef struct CANMsg CANMsg;
 
+static int CAN_std_cnt = 0;
+static int CAN_ext_cnt = 0;
+
 static uint32_t can_irq_ids[CAN_NUM] = {0};
 static can_irq_handler irq_handler;
 
@@ -121,7 +124,129 @@
 }
 
 int can_filter(can_t *obj, uint32_t id, uint32_t mask, CANFormat format, int32_t handle) {
-    return 0; // not implemented
+    uint32_t buf0, buf1;
+    int cnt1, cnt2, bound1;
+
+    /* Acceptance Filter Memory full */
+    if ((((CAN_std_cnt + 1) >> 1) + CAN_ext_cnt) >= 512)
+        return 0;                                       /* error: objects full */
+
+    /* Setup Acceptance Filter Configuration
+    Acceptance Filter Mode Register = Off  */
+    LPC_CANAF->AFMR = 0x00000001;
+
+    if (format == CANStandard)  {              /* Add mask for standard identifiers */
+        id |= (int)obj->index << 13;                        /* Add controller number */
+        id &= 0x0000F7FF;                            /* Mask out 16-bits of ID */
+
+        /* Move all remaining extended mask entries one place up
+        if new entry will increase standard ID filters list   */
+        if ((CAN_std_cnt & 0x0001) == 0 && CAN_ext_cnt != 0) {
+            cnt1   = (CAN_std_cnt >> 1);
+            bound1 = CAN_ext_cnt;
+            buf0   = LPC_CANAF_RAM->mask[cnt1];
+            while (bound1--)  {
+                cnt1++;
+                buf1 = LPC_CANAF_RAM->mask[cnt1];
+                LPC_CANAF_RAM->mask[cnt1] = buf0;
+                buf0 = buf1;
+            }
+        }
+
+        if (CAN_std_cnt == 0)  {                     /* For entering first  ID */
+            LPC_CANAF_RAM->mask[0] = 0x0000FFFF | (id << 16);
+        }  else if (CAN_std_cnt == 1)  {             /* For entering second ID */
+            if ((LPC_CANAF_RAM->mask[0] >> 16) > id)
+              LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] >> 16) | (id << 16);
+            else
+              LPC_CANAF_RAM->mask[0] = (LPC_CANAF_RAM->mask[0] & 0xFFFF0000) | id;
+        }  else  {
+            /* Find where to insert new ID */
+            cnt1 = 0;
+            cnt2 = CAN_std_cnt;
+            bound1 = (CAN_std_cnt - 1) >> 1;
+            while (cnt1 <= bound1)  {                  /* Loop through standard existing IDs */
+                if ((LPC_CANAF_RAM->mask[cnt1] >> 16) > id)  {
+                cnt2 = cnt1 * 2;
+                break;
+            }
+            if ((LPC_CANAF_RAM->mask[cnt1] & 0x0000FFFF) > id)  {
+                cnt2 = cnt1 * 2 + 1;
+                break;
+            }
+            cnt1++;                                  /* cnt1 = U32 where to insert new ID */
+        }                                          /* cnt2 = U16 where to insert new ID */
+
+        if (cnt1 > bound1)  {                      /* Adding ID as last entry */
+            if ((CAN_std_cnt & 0x0001) == 0)         /* Even number of IDs exists */
+                LPC_CANAF_RAM->mask[cnt1]  = 0x0000FFFF | (id << 16);
+            else                                     /* Odd  number of IDs exists */
+                LPC_CANAF_RAM->mask[cnt1]  = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | id;
+        } else {
+            buf0 = LPC_CANAF_RAM->mask[cnt1];        /* Remember current entry */
+            if ((cnt2 & 0x0001) == 0)                /* Insert new mask to even address */
+                buf1 = (id << 16) | (buf0 >> 16);
+            else                                     /* Insert new mask to odd  address */
+                buf1 = (buf0 & 0xFFFF0000) | id;
+
+            LPC_CANAF_RAM->mask[cnt1] = buf1;        /* Insert mask */
+
+            bound1 = CAN_std_cnt >> 1;
+            /* Move all remaining standard mask entries one place up */
+            while (cnt1 < bound1)  {
+                cnt1++;
+                buf1  = LPC_CANAF_RAM->mask[cnt1];
+                LPC_CANAF_RAM->mask[cnt1] = (buf1 >> 16) | (buf0 << 16);
+                buf0  = buf1;
+            }
+
+            if ((CAN_std_cnt & 0x0001) == 0)         /* Even number of IDs exists */
+              LPC_CANAF_RAM->mask[cnt1] = (LPC_CANAF_RAM->mask[cnt1] & 0xFFFF0000) | (0x0000FFFF);
+            }
+        }
+        CAN_std_cnt++;
+    } else {                                     /* Add mask for extended identifiers */
+        id |= (int)obj->index << 29;                 /* Add controller number */
+
+        cnt1 = ((CAN_std_cnt + 1) >> 1);
+        cnt2 = 0;
+        while (cnt2 < CAN_ext_cnt)  {                /* Loop through extended existing masks */
+            if (LPC_CANAF_RAM->mask[cnt1] > id)
+                break;
+            cnt1++;                                    /* cnt1 = U32 where to insert new mask */
+            cnt2++;
+        }
+
+        buf0 = LPC_CANAF_RAM->mask[cnt1];            /* Remember current entry */
+        LPC_CANAF_RAM->mask[cnt1] = id;              /* Insert mask */
+
+        CAN_ext_cnt++;
+
+        bound1 = CAN_ext_cnt - 1;
+        /* Move all remaining extended mask entries one place up */
+        while (cnt2 < bound1)  {
+            cnt1++;
+            cnt2++;
+            buf1 = LPC_CANAF_RAM->mask[cnt1];
+            LPC_CANAF_RAM->mask[cnt1] = buf0;
+            buf0 = buf1;
+        }
+    }
+
+    /* Calculate std ID start address (buf0) and ext ID start address (buf1) */
+    buf0 = ((CAN_std_cnt + 1) >> 1) << 2;
+    buf1 = buf0 + (CAN_ext_cnt << 2);
+
+    /* Setup acceptance filter pointers */
+    LPC_CANAF->SFF_sa     = 0;
+    LPC_CANAF->SFF_GRP_sa = buf0;
+    LPC_CANAF->EFF_sa     = buf0;
+    LPC_CANAF->EFF_GRP_sa = buf1;
+    LPC_CANAF->ENDofTable = buf1;
+
+    LPC_CANAF->AFMR = 0x00000000;                  /* Use acceptance filter */
+
+    return 1;
 }
 
 static inline void can_irq(uint32_t icr, uint32_t index) {