45 #define CAN_SEND send |
45 #define CAN_SEND send |
46 #define CAN_BIND bind |
46 #define CAN_BIND bind |
47 #define CAN_IOCTL ioctl |
47 #define CAN_IOCTL ioctl |
48 #endif |
48 #endif |
49 |
49 |
50 struct CANPort; |
|
51 #define CAN_HANDLE struct CANPort * |
|
52 |
|
53 #include <applicfg.h> |
|
54 |
|
55 #include "timer.h" |
|
56 #include "can_driver.h" |
50 #include "can_driver.h" |
57 #include "timers_driver.h" |
|
58 |
|
59 typedef struct CANPort { |
|
60 int fd; |
|
61 TASK_HANDLE receiveTask; |
|
62 CO_Data* d; |
|
63 } CANPort; |
|
64 |
51 |
65 /*********functions which permit to communicate with the board****************/ |
52 /*********functions which permit to communicate with the board****************/ |
66 UNS8 canReceive(CAN_HANDLE fd0, Message *m) |
53 UNS8 canReceive_driver(CAN_HANDLE fd0, Message *m) |
67 { |
54 { |
68 int res; |
55 int res; |
69 struct can_frame frame; |
56 struct can_frame frame; |
70 |
57 |
71 res = CAN_RECV(fd0->fd, &frame, sizeof(frame), 0); |
58 res = CAN_RECV(fd0, &frame, sizeof(frame), 0); |
72 if (res < 0) |
59 if (res < 0) |
73 return 1; |
60 return 1; |
74 |
61 |
75 m->cob_id.w = frame.can_id & CAN_EFF_MASK; |
62 m->cob_id.w = frame.can_id & CAN_EFF_MASK; |
76 m->len = frame.can_dlc; |
63 m->len = frame.can_dlc; |
81 memcpy(m->data, frame.data, 8); |
68 memcpy(m->data, frame.data, 8); |
82 |
69 |
83 return 0; |
70 return 0; |
84 } |
71 } |
85 |
72 |
86 void canReceiveLoop(CAN_HANDLE fd0) |
|
87 { |
|
88 CO_Data* d = fd0->d; |
|
89 Message m; |
|
90 |
|
91 while (1) { |
|
92 if (canReceive(fd0, &m) != 0) |
|
93 break; |
|
94 |
|
95 EnterMutex(); |
|
96 canDispatch(d, &m); |
|
97 LeaveMutex(); |
|
98 } |
|
99 } |
|
100 |
73 |
101 /***************************************************************************/ |
74 /***************************************************************************/ |
102 UNS8 canSend(CAN_HANDLE fd0, Message *m) |
75 UNS8 canSend_driver(CAN_HANDLE fd0, Message *m) |
103 { |
76 { |
104 int res; |
77 int res; |
105 struct can_frame frame; |
78 struct can_frame frame; |
106 |
79 |
107 frame.can_id = m->cob_id.w; |
80 frame.can_id = m->cob_id.w; |
111 if (m->rtr) |
84 if (m->rtr) |
112 frame.can_id |= CAN_RTR_FLAG; |
85 frame.can_id |= CAN_RTR_FLAG; |
113 else |
86 else |
114 memcpy(frame.data, m->data, 8); |
87 memcpy(frame.data, m->data, 8); |
115 |
88 |
116 res = CAN_SEND(fd0->fd, &frame, sizeof(frame), 0); |
89 res = CAN_SEND(fd0, &frame, sizeof(frame), 0); |
117 if (res < 0) |
90 if (res < 0) |
118 return 1; |
91 return 1; |
119 |
92 |
120 return 0; |
93 return 0; |
121 } |
94 } |
122 |
95 |
123 /***************************************************************************/ |
96 /***************************************************************************/ |
124 CAN_HANDLE canOpen(s_BOARD *board) |
97 CAN_HANDLE canOpen_driver(s_BOARD *board) |
125 { |
98 { |
126 CAN_HANDLE fd0; |
99 CAN_HANDLE fd0; |
127 struct ifreq ifr; |
100 struct ifreq ifr; |
128 struct sockaddr_can addr; |
101 struct sockaddr_can addr; |
129 int err; |
102 int err; |
130 |
103 |
131 fd0 = malloc(sizeof(*fd0)); |
104 fd0 = CAN_SOCKET(PF_CAN, SOCK_RAW, 0); |
132 if (!fd0) |
105 if(fd0 < 0){ |
133 return NULL; |
|
134 |
|
135 fd0->fd = CAN_SOCKET(PF_CAN, SOCK_RAW, 0); |
|
136 if(fd0->fd < 0){ |
|
137 fprintf(stderr,"Socket creation failed.\n"); |
106 fprintf(stderr,"Socket creation failed.\n"); |
138 goto error_ret; |
107 goto error_ret; |
139 } |
108 } |
140 |
109 |
141 snprintf(ifr.ifr_name, IFNAMSIZ, CAN_IFNAME, board->busname); |
110 snprintf(ifr.ifr_name, IFNAMSIZ, CAN_IFNAME, board->busname); |
142 err = CAN_IOCTL(fd0->fd, SIOCGIFINDEX, &ifr); |
111 err = CAN_IOCTL(fd0, SIOCGIFINDEX, &ifr); |
143 if (err) { |
112 if (err) { |
144 fprintf(stderr, "Unknown device: %s\n", ifr.ifr_name); |
113 fprintf(stderr, "Unknown device: %s\n", ifr.ifr_name); |
145 goto error_close; |
114 goto error_close; |
146 } |
115 } |
147 |
116 |
148 addr.can_family = AF_CAN; |
117 addr.can_family = AF_CAN; |
149 addr.can_ifindex = ifr.ifr_ifindex; |
118 addr.can_ifindex = ifr.ifr_ifindex; |
150 err = CAN_BIND(fd0->fd, (struct sockaddr *)&addr, |
119 err = CAN_BIND(fd0, (struct sockaddr *)&addr, |
151 sizeof(addr)); |
120 sizeof(addr)); |
152 if (err) { |
121 if (err) { |
153 fprintf(stderr, "Binding failed.\n"); |
122 fprintf(stderr, "Binding failed.\n"); |
154 goto error_close; |
123 goto error_close; |
155 } |
124 } |
156 |
125 |
157 fd0->d = board->d; |
|
158 CreateReceiveTask(fd0, &fd0->receiveTask); |
|
159 return fd0; |
126 return fd0; |
160 |
127 |
161 error_close: |
128 error_close: |
162 CAN_CLOSE(fd0->fd); |
129 CAN_CLOSE(fd0); |
163 |
130 |
164 error_ret: |
131 error_ret: |
165 free(fd0); |
|
166 return NULL; |
132 return NULL; |
167 } |
133 } |
168 |
134 |
169 /***************************************************************************/ |
135 /***************************************************************************/ |
170 int canClose(CAN_HANDLE fd0) |
136 int canClose_driver(CAN_HANDLE fd0) |
171 { |
137 { |
172 if (fd0) { |
138 if (fd0) { |
173 WaitReceiveTaskEnd(&fd0->receiveTask); |
139 CAN_CLOSE(fd0); |
174 CAN_CLOSE(fd0->fd); |
|
175 free(fd0); |
|
176 } |
140 } |
177 return 0; |
141 return 0; |
178 } |
142 } |