1 /*
2 * Copyright (c) 2024 NXP
3 * Copyright (c) 2025 Croxel Inc.
4 * Copyright (c) 2025 CogniPilot Foundation
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #include <zephyr/modem/ubx.h>
10 #include <zephyr/sys/check.h>
11
12 #include <zephyr/logging/log.h>
13 LOG_MODULE_REGISTER(modem_ubx, CONFIG_MODEM_MODULES_LOG_LEVEL);
14
modem_ubx_pipe_callback(struct modem_pipe * pipe,enum modem_pipe_event event,void * user_data)15 static void modem_ubx_pipe_callback(struct modem_pipe *pipe,
16 enum modem_pipe_event event,
17 void *user_data)
18 {
19 struct modem_ubx *ubx = (struct modem_ubx *)user_data;
20
21 if (event == MODEM_PIPE_EVENT_RECEIVE_READY) {
22 k_work_submit(&ubx->process_work);
23 }
24 }
25
modem_ubx_run_script(struct modem_ubx * ubx,struct modem_ubx_script * script)26 int modem_ubx_run_script(struct modem_ubx *ubx, struct modem_ubx_script *script)
27 {
28 int ret;
29 bool wait_for_rsp = script->match.filter.class != 0;
30
31 ret = k_sem_take(&ubx->script_running_sem, script->timeout);
32 if (ret != 0) {
33 return -EBUSY;
34 }
35
36 ubx->script = script;
37 k_sem_reset(&ubx->script_stopped_sem);
38
39 int tries = ubx->script->retry_count + 1;
40 int32_t ms_per_attempt = (uint64_t)k_ticks_to_ms_floor64(script->timeout.ticks) / tries;
41
42 do {
43 ret = modem_pipe_transmit(ubx->pipe,
44 (const uint8_t *)ubx->script->request.buf,
45 ubx->script->request.len);
46
47 if (wait_for_rsp) {
48 ret = k_sem_take(&ubx->script_stopped_sem, K_MSEC(ms_per_attempt));
49 }
50 tries--;
51 } while ((tries > 0) && (ret < 0));
52
53 k_sem_give(&ubx->script_running_sem);
54
55 return (ret > 0) ? 0 : ret;
56 }
57
58 enum ubx_process_result {
59 UBX_PROCESS_RESULT_NO_DATA_FOUND,
60 UBX_PROCESS_RESULT_FRAME_INCOMPLETE,
61 UBX_PROCESS_RESULT_FRAME_FOUND
62 };
63
process_incoming_data(const uint8_t * data,size_t len,const struct ubx_frame ** frame_start,size_t * frame_len,size_t * iterator)64 static inline enum ubx_process_result process_incoming_data(const uint8_t *data,
65 size_t len,
66 const struct ubx_frame **frame_start,
67 size_t *frame_len,
68 size_t *iterator)
69 {
70 for (int i = (*iterator) ; i < len ; i++) {
71 if (data[i] == UBX_PREAMBLE_SYNC_CHAR_1) {
72
73 const struct ubx_frame *frame = (const struct ubx_frame *)&data[i];
74 size_t remaining_bytes = len - i;
75
76 /* Wait until we've got the full header to keep processing data */
77 if (UBX_FRAME_HEADER_SZ > remaining_bytes) {
78 *frame_start = frame;
79 *frame_len = remaining_bytes;
80 return UBX_PROCESS_RESULT_FRAME_INCOMPLETE;
81 }
82
83 /* Filter false-positive: Sync-byte 1 contained in payload */
84 if (frame->preamble_sync_char_2 != UBX_PREAMBLE_SYNC_CHAR_2) {
85 continue;
86 }
87
88 /* Invalid length filtering */
89 if (UBX_FRAME_SZ(frame->payload_size) > UBX_FRAME_SZ_MAX) {
90 continue;
91 }
92
93 /* Check if we should wait until packet is completely received */
94 if (UBX_FRAME_SZ(frame->payload_size) > remaining_bytes) {
95 *frame_start = frame;
96 *frame_len = remaining_bytes;
97 return UBX_PROCESS_RESULT_FRAME_INCOMPLETE;
98 }
99
100 /* We should have all the packet, so we validate checksum. */
101 uint16_t valid_checksum = ubx_calc_checksum(frame,
102 UBX_FRAME_SZ(frame->payload_size));
103 uint16_t ck_a = frame->payload_and_checksum[frame->payload_size];
104 uint16_t ck_b = frame->payload_and_checksum[frame->payload_size + 1];
105 uint16_t actual_checksum = ck_a | (ck_b << 8);
106
107 if (valid_checksum != actual_checksum) {
108 continue;
109 }
110
111 *frame_start = frame;
112 *frame_len = UBX_FRAME_SZ(frame->payload_size);
113
114 *iterator = i + 1;
115 return UBX_PROCESS_RESULT_FRAME_FOUND;
116 }
117 }
118
119 return UBX_PROCESS_RESULT_NO_DATA_FOUND;
120 }
121
matches_filter(const struct ubx_frame * frame,const struct ubx_frame_match * filter)122 static inline bool matches_filter(const struct ubx_frame *frame,
123 const struct ubx_frame_match *filter)
124 {
125 if ((frame->class == filter->class) &&
126 (frame->id == filter->id) &&
127 ((filter->payload.len == 0) ||
128 ((frame->payload_size == filter->payload.len) &&
129 (0 == memcmp(frame->payload_and_checksum,
130 filter->payload.buf,
131 filter->payload.len))))) {
132 return true;
133 } else {
134 return false;
135 }
136 }
137
modem_ubx_process_handler(struct k_work * item)138 static void modem_ubx_process_handler(struct k_work *item)
139 {
140 struct modem_ubx *ubx = CONTAINER_OF(item, struct modem_ubx, process_work);
141 int ret;
142
143 ret = modem_pipe_receive(ubx->pipe,
144 &ubx->receive_buf[ubx->receive_buf_offset],
145 (ubx->receive_buf_size - ubx->receive_buf_offset));
146
147 const uint8_t *received_data = ubx->receive_buf;
148 size_t length = ret > 0 ? (ret + ubx->receive_buf_offset) : 0;
149 const struct ubx_frame *frame = NULL;
150 size_t frame_len = 0;
151 size_t iterator = 0;
152 enum ubx_process_result process_result;
153
154 do {
155 process_result = process_incoming_data(received_data, length,
156 &frame, &frame_len,
157 &iterator);
158 switch (process_result) {
159 case UBX_PROCESS_RESULT_FRAME_FOUND:
160 /** Serve script first */
161 if (matches_filter(frame, &ubx->script->match.filter)) {
162 memcpy(ubx->script->response.buf, frame, frame_len);
163 ubx->script->response.received_len = frame_len;
164
165 k_sem_give(&ubx->script_stopped_sem);
166 }
167 /** Check for unsolicited matches */
168 for (size_t i = 0 ; i < ubx->unsol_matches.size ; i++) {
169 if (ubx->unsol_matches.array[i].handler &&
170 matches_filter(frame, &ubx->unsol_matches.array[i].filter)) {
171 ubx->unsol_matches.array[i].handler(ubx, frame, frame_len,
172 ubx->user_data);
173 }
174 }
175 break;
176 case UBX_PROCESS_RESULT_FRAME_INCOMPLETE:
177 /** If we had an incomplete packet, discard prior data
178 * and offset next pipe-receive to process remaining
179 * info.
180 */
181 memcpy(ubx->receive_buf, frame, frame_len);
182 ubx->receive_buf_offset = frame_len;
183 break;
184 case UBX_PROCESS_RESULT_NO_DATA_FOUND:
185 ubx->receive_buf_offset = 0;
186 break;
187 default:
188 CODE_UNREACHABLE;
189 }
190 } while (process_result == UBX_PROCESS_RESULT_FRAME_FOUND);
191 }
192
modem_ubx_attach(struct modem_ubx * ubx,struct modem_pipe * pipe)193 int modem_ubx_attach(struct modem_ubx *ubx, struct modem_pipe *pipe)
194 {
195 if (atomic_test_and_set_bit(&ubx->attached, 0) == true) {
196 return 0;
197 }
198
199 ubx->pipe = pipe;
200 modem_pipe_attach(ubx->pipe, modem_ubx_pipe_callback, ubx);
201 k_sem_give(&ubx->script_running_sem);
202
203 return 0;
204 }
205
modem_ubx_release(struct modem_ubx * ubx)206 void modem_ubx_release(struct modem_ubx *ubx)
207 {
208 struct k_work_sync sync;
209
210 if (atomic_test_and_clear_bit(&ubx->attached, 0) == false) {
211 return;
212 }
213
214 modem_pipe_release(ubx->pipe);
215 k_work_cancel_sync(&ubx->process_work, &sync);
216 k_sem_reset(&ubx->script_stopped_sem);
217 k_sem_reset(&ubx->script_running_sem);
218 ubx->pipe = NULL;
219 }
220
modem_ubx_init(struct modem_ubx * ubx,const struct modem_ubx_config * config)221 int modem_ubx_init(struct modem_ubx *ubx, const struct modem_ubx_config *config)
222 {
223 __ASSERT_NO_MSG(ubx != NULL);
224 __ASSERT_NO_MSG(config != NULL);
225 __ASSERT_NO_MSG(config->receive_buf != NULL);
226 __ASSERT_NO_MSG(config->receive_buf_size > 0);
227
228 memset(ubx, 0x00, sizeof(*ubx));
229 ubx->user_data = config->user_data;
230
231 ubx->receive_buf = config->receive_buf;
232 ubx->receive_buf_size = config->receive_buf_size;
233
234 ubx->pipe = NULL;
235
236 ubx->unsol_matches.array = config->unsol_matches.array;
237 ubx->unsol_matches.size = config->unsol_matches.size;
238
239 k_work_init(&ubx->process_work, modem_ubx_process_handler);
240 k_sem_init(&ubx->script_stopped_sem, 0, 1);
241 k_sem_init(&ubx->script_running_sem, 1, 1);
242
243 return 0;
244 }
245