1 /*
2 * Copyright 2024 NXP
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/modem/ubx.h>
8 #include <string.h>
9
10 #include <zephyr/logging/log.h>
11 LOG_MODULE_REGISTER(modem_ubx, CONFIG_MODEM_MODULES_LOG_LEVEL);
12
13 #define MODEM_UBX_STATE_ATTACHED_BIT 0
14
modem_ubx_validate_frame_size(uint16_t ubx_frame_size,uint8_t msg_cls,uint8_t msg_id,uint16_t payload_size)15 static int modem_ubx_validate_frame_size(uint16_t ubx_frame_size, uint8_t msg_cls, uint8_t msg_id,
16 uint16_t payload_size)
17 {
18 if (ubx_frame_size > UBX_FRM_SZ_MAX ||
19 ubx_frame_size < UBX_FRM_SZ_WITHOUT_PAYLOAD ||
20 ubx_frame_size < UBX_FRM_SZ_WITHOUT_PAYLOAD + payload_size) {
21 return -1;
22 }
23
24 return 0;
25 }
26
modem_ubx_create_frame(uint8_t * ubx_frame,uint16_t ubx_frame_size,uint8_t msg_cls,uint8_t msg_id,const void * payload,uint16_t payload_size)27 int modem_ubx_create_frame(uint8_t *ubx_frame, uint16_t ubx_frame_size, uint8_t msg_cls,
28 uint8_t msg_id, const void *payload, uint16_t payload_size)
29 {
30 if (modem_ubx_validate_frame_size(ubx_frame_size, msg_cls, msg_id, payload_size)) {
31 return -1;
32 }
33
34 struct ubx_frame *frame = (struct ubx_frame *) ubx_frame;
35
36 frame->preamble_sync_char_1 = UBX_PREAMBLE_SYNC_CHAR_1;
37 frame->preamble_sync_char_2 = UBX_PREAMBLE_SYNC_CHAR_2;
38 frame->message_class = msg_cls;
39 frame->message_id = msg_id;
40 frame->payload_size_low = payload_size;
41 frame->payload_size_high = payload_size >> 8;
42
43 memcpy(frame->payload_and_checksum, payload, payload_size);
44
45 uint16_t ubx_frame_len = payload_size + UBX_FRM_SZ_WITHOUT_PAYLOAD;
46
47 uint8_t ckA = 0, ckB = 0;
48
49 for (unsigned int i = UBX_FRM_CHECKSUM_START_IDX;
50 i < (UBX_FRM_CHECKSUM_STOP_IDX(ubx_frame_len)); i++) {
51 ckA += ubx_frame[i];
52 ckB += ckA;
53 }
54
55 frame->payload_and_checksum[payload_size] = ckA;
56 frame->payload_and_checksum[payload_size + 1] = ckB;
57
58 return ubx_frame_len;
59 }
60
modem_ubx_reset_received_ubx_preamble_sync_chars(struct modem_ubx * ubx)61 static void modem_ubx_reset_received_ubx_preamble_sync_chars(struct modem_ubx *ubx)
62 {
63 ubx->ubx_preamble_sync_chars_received = false;
64 }
65
modem_ubx_reset_parser(struct modem_ubx * ubx)66 static void modem_ubx_reset_parser(struct modem_ubx *ubx)
67 {
68 modem_ubx_reset_received_ubx_preamble_sync_chars(ubx);
69 }
70
modem_ubx_get_payload_length(struct ubx_frame * frame)71 static int modem_ubx_get_payload_length(struct ubx_frame *frame)
72 {
73 uint16_t payload_len = frame->payload_size_high;
74
75 payload_len = payload_len << 8;
76
77 return payload_len | frame->payload_size_low;
78 }
79
modem_ubx_get_frame_length(struct ubx_frame * frame)80 static int modem_ubx_get_frame_length(struct ubx_frame *frame)
81 {
82 return modem_ubx_get_payload_length(frame) + UBX_FRM_SZ_WITHOUT_PAYLOAD;
83 }
84
modem_ubx_match_frame_type(struct ubx_frame * frame_1,struct ubx_frame * frame_2)85 static bool modem_ubx_match_frame_type(struct ubx_frame *frame_1, struct ubx_frame *frame_2)
86 {
87 if (frame_1->message_class == frame_2->message_class
88 && frame_1->message_id == frame_2->message_id) {
89 return true;
90 } else {
91 return false;
92 }
93 }
94
modem_ubx_match_frame_full(struct ubx_frame * frame_1,struct ubx_frame * frame_2)95 static bool modem_ubx_match_frame_full(struct ubx_frame *frame_1, struct ubx_frame *frame_2)
96 {
97 if (modem_ubx_get_frame_length(frame_1) != modem_ubx_get_frame_length(frame_2)) {
98 return false;
99 }
100
101 if (memcmp(frame_1, frame_2, modem_ubx_get_frame_length(frame_1)) == 0) {
102 return true;
103 } else {
104 return false;
105 }
106 }
107
modem_ubx_script_init(struct modem_ubx * ubx,const struct modem_ubx_script * script)108 static void modem_ubx_script_init(struct modem_ubx *ubx, const struct modem_ubx_script *script)
109 {
110 ubx->script = script;
111 }
112
modem_ubx_run_script_helper(struct modem_ubx * ubx,const struct modem_ubx_script * script)113 static int modem_ubx_run_script_helper(struct modem_ubx *ubx, const struct modem_ubx_script *script)
114 {
115 int ret;
116
117 if (ubx->pipe == NULL) {
118 return -EPERM;
119 }
120
121 k_sem_reset(&ubx->script_stopped_sem);
122
123 modem_ubx_reset_parser(ubx);
124
125 k_work_submit(&ubx->send_work);
126
127 if (ubx->script->match == NULL) {
128 return 0;
129 }
130
131 ret = k_sem_take(&ubx->script_stopped_sem, script->timeout);
132 if (ret < 0) {
133 return ret;
134 }
135
136 return 0;
137 }
138
modem_ubx_run_script(struct modem_ubx * ubx,const struct modem_ubx_script * script)139 int modem_ubx_run_script(struct modem_ubx *ubx, const struct modem_ubx_script *script)
140 {
141 int ret, attempt;
142
143 if (modem_ubx_get_frame_length(script->request) > UBX_FRM_SZ_MAX) {
144 return -EFBIG;
145 }
146
147 if (atomic_test_bit(&ubx->state, MODEM_UBX_STATE_ATTACHED_BIT) == false) {
148 return -EPERM;
149 }
150
151 ret = k_sem_take(&ubx->script_running_sem, K_FOREVER);
152 if (ret < 0) {
153 return ret;
154 }
155
156 modem_ubx_script_init(ubx, script);
157
158 for (attempt = 0; attempt < script->retry_count; ++attempt) {
159 ret = modem_ubx_run_script_helper(ubx, script);
160 if (ret > -1) {
161 LOG_INF("Successfully executed script on attempt: %d.", attempt);
162 break;
163 } else if (ret == -EPERM) {
164 break;
165 }
166 }
167
168 if (ret < 0) {
169 LOG_ERR("Failed to execute script successfully. Attempts: %d.", attempt);
170 goto unlock;
171 }
172
173 unlock:
174 k_sem_give(&ubx->script_running_sem);
175
176 return ret;
177 }
178
modem_ubx_pipe_callback(struct modem_pipe * pipe,enum modem_pipe_event event,void * user_data)179 static void modem_ubx_pipe_callback(struct modem_pipe *pipe, enum modem_pipe_event event,
180 void *user_data)
181 {
182 struct modem_ubx *ubx = (struct modem_ubx *)user_data;
183
184 if (event == MODEM_PIPE_EVENT_RECEIVE_READY) {
185 k_work_submit(&ubx->process_work);
186 }
187 }
188
modem_ubx_send_handler(struct k_work * item)189 static void modem_ubx_send_handler(struct k_work *item)
190 {
191 struct modem_ubx *ubx = CONTAINER_OF(item, struct modem_ubx, send_work);
192 int ret, tx_frame_len;
193
194 tx_frame_len = modem_ubx_get_frame_length(ubx->script->request);
195 ret = modem_pipe_transmit(ubx->pipe, (const uint8_t *) ubx->script->request, tx_frame_len);
196 if (ret < tx_frame_len) {
197 LOG_ERR("Ubx frame transmission failed. Returned %d.", ret);
198 return;
199 }
200 }
201
modem_ubx_process_received_ubx_frame(struct modem_ubx * ubx)202 static int modem_ubx_process_received_ubx_frame(struct modem_ubx *ubx)
203 {
204 int ret;
205 struct ubx_frame *received = (struct ubx_frame *) ubx->work_buf;
206
207 if (modem_ubx_match_frame_full(received, ubx->script->match) == true) {
208 /* Frame matched successfully. Terminate the script. */
209 k_sem_give(&ubx->script_stopped_sem);
210 ret = 0;
211 } else if (modem_ubx_match_frame_type(received, ubx->script->request) == true) {
212 /* Response received successfully. Script not ended. */
213 memcpy(ubx->script->response, ubx->work_buf, ubx->work_buf_len);
214 ret = -1;
215 } else {
216 /* Ignore the received frame. The device may automatically send periodic frames.
217 * These frames are not relevant for our script's execution and must be ignored.
218 */
219 ret = -1;
220 }
221
222 modem_ubx_reset_parser(ubx);
223
224 return ret;
225 }
226
modem_ubx_process_received_byte(struct modem_ubx * ubx,uint8_t byte)227 static int modem_ubx_process_received_byte(struct modem_ubx *ubx, uint8_t byte)
228 {
229 static uint8_t prev_byte;
230 static uint16_t rx_ubx_frame_len;
231
232 if (ubx->ubx_preamble_sync_chars_received == false) {
233 if (prev_byte == UBX_PREAMBLE_SYNC_CHAR_1 && byte == UBX_PREAMBLE_SYNC_CHAR_2) {
234 ubx->ubx_preamble_sync_chars_received = true;
235 ubx->work_buf[0] = UBX_PREAMBLE_SYNC_CHAR_1;
236 ubx->work_buf[1] = UBX_PREAMBLE_SYNC_CHAR_2;
237 ubx->work_buf_len = 2;
238 }
239 } else {
240 ubx->work_buf[ubx->work_buf_len] = byte;
241 ++ubx->work_buf_len;
242
243 if (ubx->work_buf_len == UBX_FRM_HEADER_SZ) {
244 uint16_t rx_ubx_payload_len = ubx->work_buf[UBX_FRM_PAYLOAD_SZ_H_IDX];
245
246 rx_ubx_payload_len = ubx->work_buf[UBX_FRM_PAYLOAD_SZ_H_IDX] << 8;
247 rx_ubx_payload_len |= ubx->work_buf[UBX_FRM_PAYLOAD_SZ_L_IDX];
248
249 rx_ubx_frame_len = rx_ubx_payload_len + UBX_FRM_SZ_WITHOUT_PAYLOAD;
250 }
251
252 if (ubx->work_buf_len == rx_ubx_frame_len) {
253 return modem_ubx_process_received_ubx_frame(ubx);
254 }
255 }
256
257 prev_byte = byte;
258
259 return -1;
260 }
261
modem_ubx_process_handler(struct k_work * item)262 static void modem_ubx_process_handler(struct k_work *item)
263 {
264 struct modem_ubx *ubx = CONTAINER_OF(item, struct modem_ubx, process_work);
265 int ret;
266
267 ret = modem_pipe_receive(ubx->pipe, ubx->receive_buf, ubx->receive_buf_size);
268 if (ret < 1) {
269 return;
270 }
271
272 const size_t length = ret;
273
274 for (int i = 0; i < length; i++) {
275 ret = modem_ubx_process_received_byte(ubx, ubx->receive_buf[i]);
276 if (ret == 0) { /* Frame matched successfully. Terminate the script. */
277 break;
278 }
279 }
280
281 k_work_submit(&ubx->process_work);
282 }
283
modem_ubx_attach(struct modem_ubx * ubx,struct modem_pipe * pipe)284 int modem_ubx_attach(struct modem_ubx *ubx, struct modem_pipe *pipe)
285 {
286 if (atomic_test_and_set_bit(&ubx->state, MODEM_UBX_STATE_ATTACHED_BIT) == true) {
287 return 0;
288 }
289
290 ubx->pipe = pipe;
291 modem_pipe_attach(ubx->pipe, modem_ubx_pipe_callback, ubx);
292 k_sem_give(&ubx->script_running_sem);
293
294 return 0;
295 }
296
modem_ubx_release(struct modem_ubx * ubx)297 void modem_ubx_release(struct modem_ubx *ubx)
298 {
299 struct k_work_sync sync;
300
301 if (atomic_test_and_clear_bit(&ubx->state, MODEM_UBX_STATE_ATTACHED_BIT) == false) {
302 return;
303 }
304
305 modem_pipe_release(ubx->pipe);
306 k_work_cancel_sync(&ubx->send_work, &sync);
307 k_work_cancel_sync(&ubx->process_work, &sync);
308 k_sem_reset(&ubx->script_stopped_sem);
309 k_sem_reset(&ubx->script_running_sem);
310 ubx->work_buf_len = 0;
311 modem_ubx_reset_parser(ubx);
312 ubx->pipe = NULL;
313 }
314
modem_ubx_init(struct modem_ubx * ubx,const struct modem_ubx_config * config)315 int modem_ubx_init(struct modem_ubx *ubx, const struct modem_ubx_config *config)
316 {
317 __ASSERT_NO_MSG(ubx != NULL);
318 __ASSERT_NO_MSG(config != NULL);
319 __ASSERT_NO_MSG(config->receive_buf != NULL);
320 __ASSERT_NO_MSG(config->receive_buf_size > 0);
321 __ASSERT_NO_MSG(config->work_buf != NULL);
322 __ASSERT_NO_MSG(config->work_buf_size > 0);
323
324 memset(ubx, 0x00, sizeof(*ubx));
325 ubx->user_data = config->user_data;
326
327 ubx->receive_buf = config->receive_buf;
328 ubx->receive_buf_size = config->receive_buf_size;
329 ubx->work_buf = config->work_buf;
330 ubx->work_buf_size = config->work_buf_size;
331
332 ubx->pipe = NULL;
333
334 k_work_init(&ubx->send_work, modem_ubx_send_handler);
335 k_work_init(&ubx->process_work, modem_ubx_process_handler);
336 k_sem_init(&ubx->script_stopped_sem, 0, 1);
337 k_sem_init(&ubx->script_running_sem, 1, 1);
338
339 return 0;
340 }
341
