1 /*
2 * Copyright (c) 2022 Martin Jäger <martin@libre.solar>
3 * Copyright (c) 2022 tado GmbH
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 #include "lorawan_services.h"
9
10 #include <zephyr/init.h>
11 #include <zephyr/kernel.h>
12 #include <zephyr/logging/log.h>
13
14 LOG_MODULE_REGISTER(lorawan_services, CONFIG_LORAWAN_SERVICES_LOG_LEVEL);
15
16 struct service_uplink_msg {
17 sys_snode_t node;
18 /* absolute ticks when this message should be scheduled */
19 int64_t ticks;
20 /* sufficient space for up to 3 answers (max 6 bytes each) */
21 uint8_t data[18];
22 uint8_t len;
23 uint8_t port;
24 bool used;
25 };
26
27 K_THREAD_STACK_DEFINE(thread_stack_area, CONFIG_LORAWAN_SERVICES_THREAD_STACK_SIZE);
28
29 /*
30 * The services need a dedicated work queue, as the LoRaWAN stack uses the system
31 * work queue and gets blocked if other LoRaWAN messages are sent and processed from
32 * the system work queue in parallel.
33 */
34 static struct k_work_q services_workq;
35
36 static struct k_work_delayable uplink_work;
37
38 /* single-linked list (with pointers) and array for implementation of priority queue */
39 static struct service_uplink_msg messages[10];
40 static sys_slist_t msg_list;
41 static struct k_sem msg_sem;
42
uplink_handler(struct k_work * work)43 static void uplink_handler(struct k_work *work)
44 {
45 struct service_uplink_msg msg_copy;
46 struct service_uplink_msg *first;
47 sys_snode_t *node;
48 int err;
49
50 ARG_UNUSED(work);
51
52 /* take semaphore and create a copy of the next message */
53 k_sem_take(&msg_sem, K_FOREVER);
54
55 node = sys_slist_get(&msg_list);
56 if (node == NULL) {
57 goto out;
58 }
59
60 first = CONTAINER_OF(node, struct service_uplink_msg, node);
61 msg_copy = *first;
62 first->used = false;
63 sys_slist_remove(&msg_list, NULL, &first->node);
64
65 /* semaphore must be given back before calling lorawan_send */
66 k_sem_give(&msg_sem);
67
68 err = lorawan_send(msg_copy.port, msg_copy.data, msg_copy.len, LORAWAN_MSG_UNCONFIRMED);
69 if (!err) {
70 LOG_DBG("Message sent to port %d", msg_copy.port);
71 } else {
72 LOG_ERR("Sending message to port %d failed: %d",
73 msg_copy.port, err);
74 }
75
76 /* take the semaphore again to schedule next uplink */
77 k_sem_take(&msg_sem, K_FOREVER);
78
79 node = sys_slist_peek_head(&msg_list);
80 if (node == NULL) {
81 goto out;
82 }
83 first = CONTAINER_OF(node, struct service_uplink_msg, node);
84 k_work_reschedule_for_queue(&services_workq, &uplink_work,
85 K_TIMEOUT_ABS_TICKS(first->ticks));
86
87 out:
88 k_sem_give(&msg_sem);
89 }
90
insert_uplink(struct service_uplink_msg * msg_new)91 static inline void insert_uplink(struct service_uplink_msg *msg_new)
92 {
93 struct service_uplink_msg *msg_prev;
94
95 if (sys_slist_is_empty(&msg_list)) {
96 sys_slist_append(&msg_list, &msg_new->node);
97 } else {
98 int count = 0;
99
100 SYS_SLIST_FOR_EACH_CONTAINER(&msg_list, msg_prev, node) {
101 count++;
102 if (msg_prev->ticks <= msg_new->ticks) {
103 break;
104 }
105 }
106 if (msg_prev != NULL) {
107 sys_slist_insert(&msg_list, &msg_prev->node, &msg_new->node);
108 } else {
109 sys_slist_append(&msg_list, &msg_new->node);
110 }
111 }
112 }
113
lorawan_services_schedule_uplink(uint8_t port,uint8_t * data,uint8_t len,uint32_t timeout)114 int lorawan_services_schedule_uplink(uint8_t port, uint8_t *data, uint8_t len, uint32_t timeout)
115 {
116 struct service_uplink_msg *next;
117 int64_t timeout_abs_ticks;
118
119 if (len > sizeof(messages[0].data)) {
120 LOG_ERR("Uplink payload for port %u too long: %u bytes", port, len);
121 LOG_HEXDUMP_ERR(data, len, "Payload: ");
122 return -EFBIG;
123 }
124
125 timeout_abs_ticks = k_uptime_ticks() + k_ms_to_ticks_ceil64(timeout);
126
127 k_sem_take(&msg_sem, K_FOREVER);
128
129 for (int i = 0; i < ARRAY_SIZE(messages); i++) {
130 if (!messages[i].used) {
131 memcpy(messages[i].data, data, len);
132 messages[i].port = port;
133 messages[i].len = len;
134 messages[i].ticks = timeout_abs_ticks;
135 messages[i].used = true;
136
137 insert_uplink(&messages[i]);
138
139 next = SYS_SLIST_PEEK_HEAD_CONTAINER(&msg_list, next, node);
140 if (next != NULL) {
141 k_work_reschedule_for_queue(&services_workq, &uplink_work,
142 K_TIMEOUT_ABS_TICKS(next->ticks));
143 }
144
145 k_sem_give(&msg_sem);
146
147 return 0;
148 }
149 }
150
151 k_sem_give(&msg_sem);
152
153 LOG_WRN("Message queue full, message for port %u dropped.", port);
154
155 return -ENOSPC;
156 }
157
lorawan_services_reschedule_work(struct k_work_delayable * dwork,k_timeout_t delay)158 int lorawan_services_reschedule_work(struct k_work_delayable *dwork, k_timeout_t delay)
159 {
160 return k_work_reschedule_for_queue(&services_workq, dwork, delay);
161 }
162
lorawan_services_init(void)163 static int lorawan_services_init(void)
164 {
165
166 sys_slist_init(&msg_list);
167 k_sem_init(&msg_sem, 1, 1);
168
169 k_work_queue_init(&services_workq);
170 k_work_queue_start(&services_workq,
171 thread_stack_area, K_THREAD_STACK_SIZEOF(thread_stack_area),
172 CONFIG_LORAWAN_SERVICES_THREAD_PRIORITY, NULL);
173
174 k_work_init_delayable(&uplink_work, uplink_handler);
175
176 k_thread_name_set(&services_workq.thread, "lorawan_services");
177
178 return 0;
179 }
180
181 SYS_INIT(lorawan_services_init, APPLICATION, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT);
182
