/
can_common.c
145 lines (115 loc) · 3.34 KB
/
can_common.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
/*
* Copyright (c) 2019 Alexander Wachter
*
* SPDX-License-Identifier: Apache-2.0
*/
#include <drivers/can.h>
#include <kernel.h>
#define LOG_LEVEL CONFIG_CAN_LOG_LEVEL
#include <logging/log.h>
LOG_MODULE_REGISTER(can_driver);
#define WORK_BUF_COUNT_IS_POWER_OF_2 !(CONFIG_CAN_WORKQ_FRAMES_BUF_CNT & \
(CONFIG_CAN_WORKQ_FRAMES_BUF_CNT - 1))
#define WORK_BUF_MOD_MASK (CONFIG_CAN_WORKQ_FRAMES_BUF_CNT - 1)
#if WORK_BUF_COUNT_IS_POWER_OF_2
#define WORK_BUF_MOD_SIZE(x) ((x) & WORK_BUF_MOD_MASK)
#else
#define WORK_BUF_MOD_SIZE(x) ((x) % CONFIG_CAN_WORKQ_FRAMES_BUF_CNT)
#endif
#define WORK_BUF_FULL 0xFFFF
static void can_msgq_put(struct zcan_frame *frame, void *arg)
{
struct k_msgq *msgq = (struct k_msgq *)arg;
int ret;
__ASSERT_NO_MSG(msgq);
ret = k_msgq_put(msgq, frame, K_NO_WAIT);
if (ret) {
LOG_ERR("Msgq %p overflowed. Frame ID: 0x%x", arg,
frame->id_type == CAN_STANDARD_IDENTIFIER ?
frame->std_id : frame->ext_id);
}
}
int z_impl_can_attach_msgq(struct device *dev, struct k_msgq *msg_q,
const struct zcan_filter *filter)
{
const struct can_driver_api *api = dev->driver_api;
return api->attach_isr(dev, can_msgq_put, msg_q, filter);
}
static inline void can_work_buffer_init(struct can_frame_buffer *buffer)
{
buffer->head = 0;
buffer->tail = 0;
}
static inline int can_work_buffer_put(struct zcan_frame *frame,
struct can_frame_buffer *buffer)
{
u16_t next_head = WORK_BUF_MOD_SIZE(buffer->head + 1);
if (buffer->head == WORK_BUF_FULL) {
return -1;
}
buffer->buf[buffer->head] = *frame;
/* Buffer is almost full */
if (next_head == buffer->tail) {
buffer->head = WORK_BUF_FULL;
} else {
buffer->head = next_head;
}
return 0;
}
static inline
struct zcan_frame *can_work_buffer_get_next(struct can_frame_buffer *buffer)
{
/* Buffer empty */
if (buffer->head == buffer->tail) {
return NULL;
} else {
return &buffer->buf[buffer->tail];
}
}
static inline void can_work_buffer_free_next(struct can_frame_buffer *buffer)
{
u16_t next_tail = WORK_BUF_MOD_SIZE(buffer->tail + 1);
if (buffer->head == buffer->tail) {
return;
}
if (buffer->head == WORK_BUF_FULL) {
buffer->head = buffer->tail;
}
buffer->tail = next_tail;
}
static void can_work_handler(struct k_work *work)
{
struct zcan_work *can_work = CONTAINER_OF(work, struct zcan_work,
work_item);
struct zcan_frame *frame;
while ((frame = can_work_buffer_get_next(&can_work->buf))) {
can_work->cb(frame, can_work->cb_arg);
can_work_buffer_free_next(&can_work->buf);
}
}
static void can_work_isr_put(struct zcan_frame *frame, void *arg)
{
struct zcan_work *work = (struct zcan_work *)arg;
int ret;
ret = can_work_buffer_put(frame, &work->buf);
if (ret) {
LOG_ERR("Workq buffer overflow. Msg ID: 0x%x",
frame->id_type == CAN_STANDARD_IDENTIFIER ?
frame->std_id : frame->ext_id);
return;
}
k_work_submit_to_queue(work->work_queue, &work->work_item);
}
int can_attach_workq(struct device *dev, struct k_work_q *work_q,
struct zcan_work *work,
can_rx_callback_t callback, void *callback_arg,
const struct zcan_filter *filter)
{
const struct can_driver_api *api = dev->driver_api;
k_work_init(&work->work_item, can_work_handler);
work->work_queue = work_q;
work->cb = callback;
work->cb_arg = callback_arg;
can_work_buffer_init(&work->buf);
return api->attach_isr(dev, can_work_isr_put, work, filter);
}