Trond Enger / d7a_1x

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Fork of d7a_1x by WizziLab

src/d7a_alp.cpp@88:dcebea09aac7, 2017-03-13 (annotated)

Committer:
Jeej
Date:
Mon Mar 13 11:58:38 2017 +0000
Revision:
88:dcebea09aac7
Parent:
87:9b885d12dbb4
Parent:
86:420918edb079 
4.6 and 4.7 locally suppoted

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jeej 43:28202405094d 1 #include "mbed.h"
Jeej 43:28202405094d 2 #include "rtos.h"
Jeej 43:28202405094d 3 #include "dbg.h"
Jeej 43:28202405094d 4 #include "d7a.h"
Jeej 43:28202405094d 5 #include "d7a_com.h"
Jeej 43:28202405094d 6 #include "d7a_common.h"
Jeej 43:28202405094d 7 #include "d7a_fs.h"
Jeej 43:28202405094d 8 #include "d7a_modem.h"
Jeej 43:28202405094d 9 #include "d7a_sys.h"
Jeej 43:28202405094d 10 #include "d7a_alp.h"
Jeej 67:9ac9d109b80a 11 #include "sha.h"
Jeej 84:e5388f1b8ed9 12 #include "d7a_typedefs.h"
Jeej 43:28202405094d 13
Jeej 86:420918edb079 14 #if 1
Jeej 79:82b01c1a62f6 15 #define ALP_DPRINT(...) DPRINT(__VA_ARGS__)
Jeej 85:197f67940c0d 16 #define ALP_DPRINT_DATA(...) DPRINT_DATA(__VA_ARGS__)
Jeej 83:f4054d0b29ba 17 #define ALP_FPRINT(...) FPRINT(__VA_ARGS__)
Jeej 79:82b01c1a62f6 18 #else
Jeej 79:82b01c1a62f6 19 #define ALP_DPRINT(...);
Jeej 79:82b01c1a62f6 20 #define ALP_DPRINT_DATA(...);
Jeej 83:f4054d0b29ba 21 #define ALP_FPRINT(...);
Jeej 79:82b01c1a62f6 22 #endif
Jeej 79:82b01c1a62f6 23
Jeej 77:8c792719a1fc 24 #define ALP_CMD_MAX_LENGHT (256)
Jeej 85:197f67940c0d 25 #define MAX_RESPONSES (32)
Jeej 43:28202405094d 26
Jeej 77:8c792719a1fc 27 static uint8_t g_alp_tag;
Jeej 87:9b885d12dbb4 28 static uint8_t g_alp_root_key_size;
Jeej 77:8c792719a1fc 29 static uint8_t g_alp_buffer[ALP_CMD_MAX_LENGHT];
Jeej 77:8c792719a1fc 30 static UnsolicitedMsgFunction g_alp_uns_msg;
Jeej 84:e5388f1b8ed9 31
Jeej 84:e5388f1b8ed9 32 static OS_Queue<d7a_com_rx_msg_t, 8> g_alp_pkt_queue;
Jeej 84:e5388f1b8ed9 33 static OS_Queue<d7a_com_rx_msg_t, 8> g_alp_pl_queue;
Jeej 85:197f67940c0d 34 static OS_Thread g_alp_thread(osPriorityHigh, 512, NULL);
Jeej 77:8c792719a1fc 35
Jeej 76:fda2e34ff19d 36 void d7a_alp_thread();
Jeej 43:28202405094d 37
Jeej 59:b42eae56b51b 38 d7a_errors_t d7a_alp_open(UnsolicitedMsgFunction uns_msg)
Jeej 43:28202405094d 39 {
Jeej 83:f4054d0b29ba 40 ALP_FPRINT("\r\n");
Jeej 43:28202405094d 41
Jeej 87:9b885d12dbb4 42 g_alp_uns_msg = uns_msg;
Jeej 87:9b885d12dbb4 43 g_alp_root_key_size = D7A_ROOT_KEY_SIZE;
Jeej 76:fda2e34ff19d 44
Jeej 77:8c792719a1fc 45 osStatus err = g_alp_thread.start(d7a_alp_thread);
Jeej 76:fda2e34ff19d 46 ASSERT(err == osOK, "Failed to start d7a_alp_thread (err: %d)\r\n", err);
Jeej 49:81d5bddb02f0 47
Jeej 49:81d5bddb02f0 48 return D7A_ERR_NONE;
Jeej 43:28202405094d 49 }
Jeej 43:28202405094d 50
Jeej 56:da34fc11e760 51 d7a_errors_t d7a_alp_close(void)
Jeej 56:da34fc11e760 52 {
Jeej 83:f4054d0b29ba 53 ALP_FPRINT("\r\n");
Jeej 56:da34fc11e760 54
Jeej 77:8c792719a1fc 55 g_alp_thread.terminate();
Jeej 56:da34fc11e760 56
Jeej 56:da34fc11e760 57 return D7A_ERR_NONE;
Jeej 56:da34fc11e760 58 }
Jeej 56:da34fc11e760 59
Jeej 43:28202405094d 60 void d7a_alp_new_pkt(d7a_com_rx_msg_t* pkt)
Jeej 43:28202405094d 61 {
Jeej 83:f4054d0b29ba 62 ALP_FPRINT("\r\n");
Jeej 77:8c792719a1fc 63 ASSERT(g_alp_pkt_queue.put(pkt) == osOK, "ALP queue full!\r\n");
Jeej 43:28202405094d 64 }
Jeej 43:28202405094d 65
Jeej 67:9ac9d109b80a 66 static void d7a_alp_new_pl(d7a_com_rx_msg_t* pl)
Jeej 45:b85384e7d825 67 {
Jeej 83:f4054d0b29ba 68 ALP_FPRINT("\r\n");
Jeej 77:8c792719a1fc 69 ASSERT(g_alp_pl_queue.put(pl) == osOK, "ALP PL queue full!\r\n");
Jeej 45:b85384e7d825 70 }
Jeej 45:b85384e7d825 71
Jeej 67:9ac9d109b80a 72 static d7a_com_rx_msg_t* d7a_alp_wait_pkt(uint32_t millisec)
Jeej 43:28202405094d 73 {
Jeej 83:f4054d0b29ba 74 ALP_FPRINT("\r\n");
Jeej 77:8c792719a1fc 75 osEvent evt = g_alp_pkt_queue.get(millisec);
Jeej 43:28202405094d 76 return (evt.status == osEventMessage)? (d7a_com_rx_msg_t*)evt.value.p : NULL;
Jeej 43:28202405094d 77 }
Jeej 43:28202405094d 78
Jeej 67:9ac9d109b80a 79 static d7a_com_rx_msg_t* d7a_alp_wait_pl(uint32_t millisec)
Jeej 43:28202405094d 80 {
Jeej 83:f4054d0b29ba 81 ALP_FPRINT("\r\n");
Jeej 77:8c792719a1fc 82 osEvent evt = g_alp_pl_queue.get(millisec);
Jeej 45:b85384e7d825 83 return (evt.status == osEventMessage)? (d7a_com_rx_msg_t*)evt.value.p : NULL;
Jeej 43:28202405094d 84 }
Jeej 43:28202405094d 85
Jeej 58:38a366236bda 86 static uint32_t d7a_ctf_to_ti(d7a_ctf_t ctf)
Jeej 58:38a366236bda 87 {
Jeej 58:38a366236bda 88 return ((1 << (2*ctf.bf.exp)) * ctf.bf.mant);
Jeej 58:38a366236bda 89 }
Jeej 58:38a366236bda 90
Jeej 67:9ac9d109b80a 91 static uint32_t d7a_alp_encode_length(uint8_t* p, uint32_t len)
Jeej 43:28202405094d 92 {
Jeej 43:28202405094d 93 if (len <= 0x3F)
Jeej 43:28202405094d 94 {
Jeej 43:28202405094d 95 *p++ = len;
Jeej 43:28202405094d 96 return 1;
Jeej 43:28202405094d 97 }
Jeej 43:28202405094d 98 else if (len <= 0x3FFF)
Jeej 43:28202405094d 99 {
Jeej 43:28202405094d 100 *p++ = 0x40 + (uint8_t)(len >> 8);
Jeej 43:28202405094d 101 *p++ = (uint8_t)(len & 0xFF);
Jeej 43:28202405094d 102 return 2;
Jeej 43:28202405094d 103 }
Jeej 43:28202405094d 104 else if (len <= 0x3FFFFF)
Jeej 43:28202405094d 105 {
Jeej 43:28202405094d 106 *p++ = 0x80 + (uint8_t) (len >> 16);
Jeej 43:28202405094d 107 *p++ = (uint8_t)((len >> 8) & 0xFF);
Jeej 43:28202405094d 108 *p++ = (uint8_t) (len & 0xFF);
Jeej 43:28202405094d 109 return 3;
Jeej 43:28202405094d 110 }
Jeej 43:28202405094d 111 else
Jeej 43:28202405094d 112 {
Jeej 43:28202405094d 113 *p++ = 0xC0 + (uint8_t) (len >> 24);
Jeej 43:28202405094d 114 *p++ = (uint8_t)((len >> 16) & 0xFF);
Jeej 43:28202405094d 115 *p++ = (uint8_t)((len >> 8) & 0xFF);
Jeej 43:28202405094d 116 *p++ = (uint8_t) (len & 0xFF);
Jeej 43:28202405094d 117 return 4;
Jeej 43:28202405094d 118 }
Jeej 43:28202405094d 119 }
Jeej 43:28202405094d 120
Jeej 67:9ac9d109b80a 121 static uint32_t alp_decode_length(uint8_t* p, uint32_t* len)
Jeej 45:b85384e7d825 122 {
Jeej 45:b85384e7d825 123 uint32_t tmp = 0;
Jeej 45:b85384e7d825 124 switch ((*p) & 0xC0)
Jeej 45:b85384e7d825 125 {
Jeej 45:b85384e7d825 126 case 0xC0: // 0xCx xx xx xx
Jeej 45:b85384e7d825 127 tmp = (*p++ & 0x3F) << 24;
Jeej 45:b85384e7d825 128 tmp += *p++ << 16;
Jeej 45:b85384e7d825 129 tmp += *p++ << 8;
Jeej 45:b85384e7d825 130 tmp += *p++ << 0;
Jeej 45:b85384e7d825 131 *len = tmp;
Jeej 45:b85384e7d825 132 return 4;
Jeej 45:b85384e7d825 133 case 0x80: // 0x8x xx xx : 16384 <= Len <4194303
Jeej 45:b85384e7d825 134 tmp = (*p++ & 0x3F) << 16;
Jeej 45:b85384e7d825 135 tmp += *p++ << 8;
Jeej 45:b85384e7d825 136 if (tmp == 0) {
Jeej 45:b85384e7d825 137 // 0x8000 ActP special ActP code
Jeej 45:b85384e7d825 138 // Do not fetch the extra byte
Jeej 45:b85384e7d825 139 tmp = 2;
Jeej 45:b85384e7d825 140 }
Jeej 45:b85384e7d825 141 else
Jeej 45:b85384e7d825 142 {
Jeej 45:b85384e7d825 143 tmp += *p++ << 0;
Jeej 45:b85384e7d825 144 }
Jeej 45:b85384e7d825 145 *len = tmp;
Jeej 45:b85384e7d825 146 return 3;
Jeej 45:b85384e7d825 147 case 0x40: // 0x4x xx : 64 <= Len < 16383
Jeej 45:b85384e7d825 148 tmp = (*p++ & 0x3F) << 8;
Jeej 45:b85384e7d825 149 tmp += *p++ << 0;
Jeej 45:b85384e7d825 150 if (tmp == 0)
Jeej 45:b85384e7d825 151 {
Jeej 45:b85384e7d825 152 // 0x4000 ActP special ActP code
Jeej 45:b85384e7d825 153 tmp = 1;
Jeej 45:b85384e7d825 154 }
Jeej 45:b85384e7d825 155 *len = tmp;
Jeej 45:b85384e7d825 156 return 2;
Jeej 45:b85384e7d825 157 case 0: // Len <63
Jeej 45:b85384e7d825 158 tmp = (*p++ & 0x3F) << 0;
Jeej 45:b85384e7d825 159 *len = tmp;
Jeej 45:b85384e7d825 160 return 1;
Jeej 45:b85384e7d825 161 }
Jeej 45:b85384e7d825 162
Jeej 45:b85384e7d825 163 return 0;
Jeej 45:b85384e7d825 164 }
Jeej 43:28202405094d 165
Jeej 67:9ac9d109b80a 166 static uint32_t d7a_alp_add(uint8_t* p, const uint8_t* data, uint32_t len)
Jeej 43:28202405094d 167 {
Jeej 45:b85384e7d825 168 memcpy(p, data, len);
Jeej 43:28202405094d 169
Jeej 43:28202405094d 170 return len;
Jeej 43:28202405094d 171 }
Jeej 43:28202405094d 172
Jeej 59:b42eae56b51b 173 void d7a_alp_free_msg(d7a_msg_t* msg)
Jeej 59:b42eae56b51b 174 {
Jeej 83:f4054d0b29ba 175 ALP_FPRINT("\r\n");
Jeej 67:9ac9d109b80a 176
Jeej 59:b42eae56b51b 177 if (msg->data)
Jeej 59:b42eae56b51b 178 {
Jeej 59:b42eae56b51b 179 FREE(msg->data);
Jeej 59:b42eae56b51b 180 }
Jeej 59:b42eae56b51b 181 FREE(msg);
Jeej 59:b42eae56b51b 182 }
Jeej 59:b42eae56b51b 183
Jeej 67:9ac9d109b80a 184 static d7a_msg_t* d7a_alp_new_msg(void)
Jeej 59:b42eae56b51b 185 {
Jeej 83:f4054d0b29ba 186 ALP_FPRINT("\r\n");
Jeej 67:9ac9d109b80a 187
Jeej 59:b42eae56b51b 188 d7a_msg_t* msg = (d7a_msg_t*)MALLOC(sizeof(d7a_msg_t));
Jeej 59:b42eae56b51b 189 memset(msg, 0, sizeof(d7a_msg_t));
Jeej 59:b42eae56b51b 190 msg->err = D7A_ERR_NONE;
Jeej 59:b42eae56b51b 191
Jeej 59:b42eae56b51b 192 return msg;
Jeej 59:b42eae56b51b 193 }
Jeej 58:38a366236bda 194
Jeej 67:9ac9d109b80a 195 static d7a_alp_rsp_t* d7a_alp_parse_pl(d7a_com_rx_msg_t* pkt)
Jeej 67:9ac9d109b80a 196 {
Jeej 83:f4054d0b29ba 197 ALP_FPRINT("\r\n");
Jeej 67:9ac9d109b80a 198
Jeej 46:665391110051 199 if (pkt == NULL)
Jeej 46:665391110051 200 {
Jeej 46:665391110051 201 return NULL;
Jeej 46:665391110051 202 }
Jeej 46:665391110051 203
Jeej 45:b85384e7d825 204 uint8_t* p = pkt->buffer;
Jeej 45:b85384e7d825 205 uint8_t* t = p;
Jeej 45:b85384e7d825 206 uint8_t len = pkt->blen;
Jeej 45:b85384e7d825 207
Jeej 45:b85384e7d825 208 d7a_alp_rsp_t* rsp = (d7a_alp_rsp_t*)MALLOC(sizeof(d7a_alp_rsp_t));
Jeej 59:b42eae56b51b 209 rsp->tag = NO_TAG;
Jeej 59:b42eae56b51b 210 rsp->eop = false;
Jeej 59:b42eae56b51b 211 rsp->msg = d7a_alp_new_msg();
Jeej 45:b85384e7d825 212
Jeej 45:b85384e7d825 213 while ((p - t) < len)
Jeej 45:b85384e7d825 214 {
Jeej 45:b85384e7d825 215 uint8_t ctrl = *p++;
Jeej 45:b85384e7d825 216 switch (ctrl & 0x3F)
Jeej 45:b85384e7d825 217 {
Jeej 45:b85384e7d825 218 case ALP_OPCODE_RSP_STATUS:
Jeej 46:665391110051 219 if (ctrl & ALP_OPCODE_INDIRECT)
Jeej 46:665391110051 220 {
Jeej 58:38a366236bda 221 // ITF Type
Jeej 58:38a366236bda 222 uint8_t type = *p++;
Jeej 58:38a366236bda 223 // Length
Jeej 46:665391110051 224 uint32_t length;
Jeej 58:38a366236bda 225 p += alp_decode_length(p, &length);
Jeej 58:38a366236bda 226 // Data
Jeej 58:38a366236bda 227 d7a_sp_res_t* res = (d7a_sp_res_t*)p;
Jeej 58:38a366236bda 228 p += length;
Jeej 58:38a366236bda 229
Jeej 58:38a366236bda 230 // Fill corresponding fields
Jeej 59:b42eae56b51b 231 rsp->msg->lb = res->lb; // Get Link Budget
Jeej 61:87be16080640 232 rsp->msg->rxlev = res->rxlev; // Get RXLEV
Jeej 59:b42eae56b51b 233 memcpy(rsp->msg->id, res->addressee.id, D7A_UID_LEN); // Get UID
Jeej 58:38a366236bda 234
Jeej 79:82b01c1a62f6 235 ALP_DPRINT("ALP RSP ISTATUS type:%02X lb: %3d ", type, rsp->msg->lb);
Jeej 80:30265d5dd20a 236 ALP_DPRINT_DATA("UID:", "%02X", rsp->msg->id, D7A_UID_LEN, "\r\n");
Jeej 46:665391110051 237 }
Jeej 46:665391110051 238 else
Jeej 46:665391110051 239 {
Jeej 59:b42eae56b51b 240 uint8_t aid = *p++; // Action ID
Jeej 59:b42eae56b51b 241 rsp->msg->err = *p++; // Status
Jeej 79:82b01c1a62f6 242 ALP_DPRINT("ALP RSP STATUS aid:%d Status:%d\r\n", aid, rsp->msg->err);
Jeej 46:665391110051 243 }
Jeej 45:b85384e7d825 244 break;
Jeej 45:b85384e7d825 245 case ALP_OPCODE_RSP_TAG:
Jeej 59:b42eae56b51b 246 rsp->eop = !!(ctrl & ALP_CTRL_EOP);
Jeej 45:b85384e7d825 247 rsp->tag = *p++; // TAG
Jeej 79:82b01c1a62f6 248 ALP_DPRINT("ALP RSP TAG %d EOP %d\r\n", rsp->tag, rsp->eop);
Jeej 45:b85384e7d825 249 break;
Jeej 45:b85384e7d825 250 case ALP_OPCODE_RSP_F_DATA:
Jeej 59:b42eae56b51b 251 uint8_t fid;
Jeej 45:b85384e7d825 252 uint32_t offset;
Jeej 59:b42eae56b51b 253 uint32_t length;
Jeej 59:b42eae56b51b 254 fid = *p++; // File ID
Jeej 45:b85384e7d825 255 p += alp_decode_length(p, &offset); // offset
Jeej 59:b42eae56b51b 256 p += alp_decode_length(p, &length); // length
Jeej 59:b42eae56b51b 257 rsp->msg->data = (d7a_data_t*)MALLOC(sizeof(d7a_data_t) - 1 + length);
Jeej 59:b42eae56b51b 258 rsp->msg->data->fid = fid;
Jeej 59:b42eae56b51b 259 rsp->msg->data->offset = offset;
Jeej 59:b42eae56b51b 260 rsp->msg->data->length = length;
Jeej 59:b42eae56b51b 261 p += d7a_alp_add(rsp->msg->data->buf, p, length);
Jeej 79:82b01c1a62f6 262 ALP_DPRINT("ALP RSP F_DATA f:%d o:%d s:%d\r\n", fid, offset, length);
Jeej 80:30265d5dd20a 263 //ALP_DPRINT_DATA("DATA: ", "%02X ", (uint8_t*)rsp->data, rsp->data_len, "\r\n");
Jeej 45:b85384e7d825 264 break;
Jeej 45:b85384e7d825 265 default:
Jeej 45:b85384e7d825 266 WARNING(false, "ALP Untreated OP %d\r\n", ctrl);
Jeej 45:b85384e7d825 267 break;
Jeej 45:b85384e7d825 268 }
Jeej 45:b85384e7d825 269 }
Jeej 45:b85384e7d825 270
Jeej 45:b85384e7d825 271 ASSERT((p - t) == len, "Payload wrong size: %d expected %d\r\n", (p - t), len);
Jeej 67:9ac9d109b80a 272
Jeej 45:b85384e7d825 273 return rsp;
Jeej 45:b85384e7d825 274 }
Jeej 45:b85384e7d825 275
Jeej 45:b85384e7d825 276
Jeej 67:9ac9d109b80a 277 static uint32_t d7a_alp_tag(uint8_t* p, bool eop)
Jeej 43:28202405094d 278 {
Jeej 43:28202405094d 279 uint8_t* t = p;
Jeej 43:28202405094d 280
Jeej 45:b85384e7d825 281 *p++ = ALP_OPCODE_TAG + ((eop)? ALP_CTRL_EOP : 0);
Jeej 77:8c792719a1fc 282 *p++ = ++g_alp_tag;
Jeej 43:28202405094d 283
Jeej 43:28202405094d 284 return (uint32_t)(p - t);
Jeej 43:28202405094d 285 }
Jeej 43:28202405094d 286
Jeej 71:f03727ff0f99 287 static uint32_t d7a_alp_forward_action(uint8_t* p, d7a_itf_t* itf, bool resp)
Jeej 43:28202405094d 288 {
Jeej 43:28202405094d 289 uint8_t* t = p;
Jeej 43:28202405094d 290
Jeej 71:f03727ff0f99 291 uint32_t itf_size = sizeof(d7a_itf_t);
Jeej 58:38a366236bda 292 switch (itf->cfg.addressee.ctrl.bf.idf)
Jeej 58:38a366236bda 293 {
Jeej 58:38a366236bda 294 case D7A_ID_NBID:
Jeej 58:38a366236bda 295 itf_size -= 7;
Jeej 58:38a366236bda 296 break;
Jeej 58:38a366236bda 297 case D7A_ID_NOID:
Jeej 58:38a366236bda 298 itf_size -= 8;
Jeej 58:38a366236bda 299 break;
Jeej 58:38a366236bda 300 case D7A_ID_UID:
Jeej 58:38a366236bda 301 break;
Jeej 58:38a366236bda 302 case D7A_ID_VID:
Jeej 58:38a366236bda 303 itf_size -= 4;
Jeej 58:38a366236bda 304 break;
Jeej 58:38a366236bda 305 default:
Jeej 58:38a366236bda 306 break;
Jeej 58:38a366236bda 307 }
Jeej 58:38a366236bda 308
Jeej 45:b85384e7d825 309 *p++ = ALP_OPCODE_FORWARD + ((resp)? ALP_CTRL_RESP : 0);
Jeej 58:38a366236bda 310 p += d7a_alp_add(p, (uint8_t*)itf, itf_size);
Jeej 43:28202405094d 311
Jeej 43:28202405094d 312 return (uint32_t)(p - t);
Jeej 43:28202405094d 313 }
Jeej 43:28202405094d 314
Jeej 67:9ac9d109b80a 315 static uint32_t d7a_alp_write_action(uint8_t* p, const uint8_t file_id, const uint32_t offset, const uint32_t size, const uint8_t* const buf, bool resp)
Jeej 43:28202405094d 316 {
Jeej 43:28202405094d 317 uint8_t* t = p;
Jeej 43:28202405094d 318
Jeej 45:b85384e7d825 319 *p++ = ALP_OPCODE_F_WR_DATA + ((resp)? ALP_CTRL_RESP : 0);
Jeej 43:28202405094d 320 *p++ = file_id;
Jeej 43:28202405094d 321 p += d7a_alp_encode_length(p, offset);
Jeej 43:28202405094d 322 p += d7a_alp_encode_length(p, size);
Jeej 43:28202405094d 323 p += d7a_alp_add(p, buf, size);
Jeej 43:28202405094d 324
Jeej 43:28202405094d 325 return (uint32_t)(p - t);
Jeej 43:28202405094d 326 }
Jeej 43:28202405094d 327
Jeej 67:9ac9d109b80a 328 static uint32_t d7a_alp_read_action(uint8_t* p, const uint8_t file_id, const uint32_t offset, const uint32_t size, bool resp)
Jeej 43:28202405094d 329 {
Jeej 43:28202405094d 330 uint8_t* t = p;
Jeej 43:28202405094d 331
Jeej 45:b85384e7d825 332 *p++ = ALP_OPCODE_F_RD_DATA + ((resp)? ALP_CTRL_RESP : 0);
Jeej 43:28202405094d 333 *p++ = file_id;
Jeej 43:28202405094d 334 p += d7a_alp_encode_length(p, offset);
Jeej 43:28202405094d 335 p += d7a_alp_encode_length(p, size);
Jeej 43:28202405094d 336
Jeej 43:28202405094d 337 return (uint32_t)(p - t);
Jeej 43:28202405094d 338 }
Jeej 43:28202405094d 339
Jeej 67:9ac9d109b80a 340 static uint32_t d7a_alp_perm_request_action(uint8_t* p, uint8_t* req, uint32_t req_size, const uint8_t* root_key, bool resp)
Jeej 43:28202405094d 341 {
Jeej 67:9ac9d109b80a 342 uint8_t* t = p;
Jeej 67:9ac9d109b80a 343 uint8_t hash[32];
Jeej 67:9ac9d109b80a 344
Jeej 67:9ac9d109b80a 345 *p++ = ALP_OPCODE_PERM_REQ + ((resp)? ALP_CTRL_RESP : 0);
Jeej 67:9ac9d109b80a 346 *p++ = 1; // ROOT request
Jeej 67:9ac9d109b80a 347 *p++ = 42; // Auth protocol ID
Jeej 67:9ac9d109b80a 348 sha256_init();
Jeej 67:9ac9d109b80a 349 sha256_update(req, req_size);
Jeej 87:9b885d12dbb4 350 sha256_update((uint8_t*)root_key, g_alp_root_key_size);
Jeej 67:9ac9d109b80a 351 sha256_final(hash);
Jeej 82:5d1b101b6d11 352 //PRINT_DATA("Req : ", "%02X ", (uint8_t*)req, req_size, "\r\n");
Jeej 87:9b885d12dbb4 353 //PRINT_DATA("Key : ", "%d ", (uint8_t*)root_key, g_alp_root_key_size, "\r\n");
Jeej 67:9ac9d109b80a 354 //PRINT_DATA("Token: ", "%02X", hash, D7A_AUTH_PROTOCOLE_TOKEN_SIZE, "\r\n");
Jeej 67:9ac9d109b80a 355 p += d7a_alp_add(p, hash, D7A_AUTH_PROTOCOLE_TOKEN_SIZE);
Jeej 67:9ac9d109b80a 356
Jeej 67:9ac9d109b80a 357 return (uint32_t)(p - t);
Jeej 67:9ac9d109b80a 358 }
Jeej 67:9ac9d109b80a 359
Jeej 67:9ac9d109b80a 360 static uint32_t d7a_alp_flush_action(uint8_t* p, uint8_t fid, bool resp)
Jeej 67:9ac9d109b80a 361 {
Jeej 67:9ac9d109b80a 362 uint8_t* t = p;
Jeej 67:9ac9d109b80a 363
Jeej 67:9ac9d109b80a 364 *p++ = ALP_OPCODE_F_FLUSH + ((resp)? ALP_CTRL_RESP : 0);
Jeej 67:9ac9d109b80a 365 *p++ = fid;
Jeej 67:9ac9d109b80a 366
Jeej 67:9ac9d109b80a 367 return (uint32_t)(p - t);
Jeej 67:9ac9d109b80a 368 }
Jeej 67:9ac9d109b80a 369
Jeej 85:197f67940c0d 370 static void d7a_alp_construct_resp(d7a_msg_t** ret, uint8_t current_tag)
Jeej 67:9ac9d109b80a 371 {
Jeej 83:f4054d0b29ba 372 ALP_FPRINT("\r\n");
Jeej 67:9ac9d109b80a 373
Jeej 59:b42eae56b51b 374 int i = 0;
Jeej 59:b42eae56b51b 375 d7a_alp_rsp_t* pl = NULL;
Jeej 59:b42eae56b51b 376 d7a_com_rx_msg_t* pkt = NULL;
Jeej 70:07b378285c95 377 int32_t time;
Jeej 70:07b378285c95 378 Timer timeout;
Jeej 70:07b378285c95 379
Jeej 70:07b378285c95 380 timeout.start();
Jeej 59:b42eae56b51b 381
Jeej 59:b42eae56b51b 382 // Parse responses
Jeej 59:b42eae56b51b 383 do
Jeej 43:28202405094d 384 {
Jeej 70:07b378285c95 385 time = D7A_ALP_RESP_TO - timeout.read_ms();
Jeej 70:07b378285c95 386 if (time < 0) time = 0;
Jeej 70:07b378285c95 387
Jeej 70:07b378285c95 388 pkt = d7a_alp_wait_pl(time);
Jeej 59:b42eae56b51b 389
Jeej 59:b42eae56b51b 390 if (pkt == NULL)
Jeej 59:b42eae56b51b 391 {
Jeej 59:b42eae56b51b 392 ret[i] = d7a_alp_new_msg();
Jeej 59:b42eae56b51b 393 ret[i]->err = D7A_ERR_CMD_TO;
Jeej 59:b42eae56b51b 394 break;
Jeej 59:b42eae56b51b 395 }
Jeej 59:b42eae56b51b 396
Jeej 59:b42eae56b51b 397 pl = d7a_alp_parse_pl(pkt);
Jeej 67:9ac9d109b80a 398
Jeej 59:b42eae56b51b 399 // Check TAG
Jeej 59:b42eae56b51b 400 if (pl->tag == NO_TAG)
Jeej 59:b42eae56b51b 401 {
Jeej 59:b42eae56b51b 402 WARNING(false, "No tag in payload expected %d\r\n", current_tag);
Jeej 59:b42eae56b51b 403 FREE(pkt);
Jeej 59:b42eae56b51b 404 d7a_alp_free_msg(pl->msg);
Jeej 59:b42eae56b51b 405 FREE(pl);
Jeej 59:b42eae56b51b 406 ret[i] = d7a_alp_new_msg();
Jeej 59:b42eae56b51b 407 ret[i]->err = D7A_ERR_UNKNOWN;
Jeej 59:b42eae56b51b 408 break;
Jeej 59:b42eae56b51b 409 }
Jeej 59:b42eae56b51b 410
Jeej 59:b42eae56b51b 411 if (pl->tag != current_tag)
Jeej 59:b42eae56b51b 412 {
Jeej 59:b42eae56b51b 413 WARNING(false, "Ingnoring tag %d expecting %d\r\n", pl->tag, current_tag);
Jeej 59:b42eae56b51b 414 d7a_alp_free_msg(pl->msg);
Jeej 59:b42eae56b51b 415 FREE(pl);
Jeej 59:b42eae56b51b 416 d7a_alp_new_pl(pkt);
Jeej 59:b42eae56b51b 417 continue;
Jeej 59:b42eae56b51b 418 }
Jeej 59:b42eae56b51b 419
Jeej 59:b42eae56b51b 420 FREE(pkt);
Jeej 59:b42eae56b51b 421
Jeej 59:b42eae56b51b 422 // Check for END OF PAYLOAD
Jeej 59:b42eae56b51b 423 if (pl->eop)
Jeej 59:b42eae56b51b 424 {
Jeej 79:82b01c1a62f6 425 ALP_DPRINT("EOP\r\n");
Jeej 59:b42eae56b51b 426
Jeej 59:b42eae56b51b 427 // If tag only
Jeej 62:f1e4da5162c2 428 if (!pl->msg->data && !pl->msg->lb && i != 0)
Jeej 59:b42eae56b51b 429 {
Jeej 59:b42eae56b51b 430 // Ignore response
Jeej 59:b42eae56b51b 431 d7a_alp_free_msg(pl->msg);
Jeej 59:b42eae56b51b 432 }
Jeej 59:b42eae56b51b 433 else
Jeej 59:b42eae56b51b 434 {
Jeej 79:82b01c1a62f6 435 ALP_DPRINT("last response (err %d)\r\n", pl->msg->err);
Jeej 59:b42eae56b51b 436 ret[i] = pl->msg;
Jeej 59:b42eae56b51b 437 }
Jeej 59:b42eae56b51b 438
Jeej 59:b42eae56b51b 439 FREE(pl);
Jeej 59:b42eae56b51b 440 break;
Jeej 59:b42eae56b51b 441 }
Jeej 59:b42eae56b51b 442 // Wait for new msg
Jeej 59:b42eae56b51b 443 else
Jeej 59:b42eae56b51b 444 {
Jeej 79:82b01c1a62f6 445 ALP_DPRINT("next response (err %d)\r\n", pl->msg->err);
Jeej 85:197f67940c0d 446
Jeej 85:197f67940c0d 447 if (i > MAX_RESPONSES)
Jeej 85:197f67940c0d 448 {
Jeej 85:197f67940c0d 449 WARNING(false, "Too much responses! max: %d\r\n", MAX_RESPONSES);
Jeej 85:197f67940c0d 450 FREE(pl->msg);
Jeej 85:197f67940c0d 451 }
Jeej 85:197f67940c0d 452 else
Jeej 85:197f67940c0d 453 {
Jeej 85:197f67940c0d 454 ret[i] = pl->msg;
Jeej 85:197f67940c0d 455 i++;
Jeej 85:197f67940c0d 456 }
Jeej 85:197f67940c0d 457
Jeej 59:b42eae56b51b 458 FREE(pl);
Jeej 59:b42eae56b51b 459 }
Jeej 59:b42eae56b51b 460
Jeej 59:b42eae56b51b 461 } while (1);
Jeej 58:38a366236bda 462 }
Jeej 45:b85384e7d825 463
Jeej 85:197f67940c0d 464 static uint32_t d7a_alp_construct_itf(uint8_t* p, d7a_addressee_t* addressee, uint8_t retry, bool resp)
Jeej 43:28202405094d 465 {
Jeej 58:38a366236bda 466 bool broadcast = false;
Jeej 58:38a366236bda 467
Jeej 58:38a366236bda 468 if (addressee)
Jeej 58:38a366236bda 469 {
Jeej 85:197f67940c0d 470 if (addressee->ctrl.bf.idf == D7A_ID_NBID || addressee->ctrl.bf.idf == D7A_ID_NOID)
Jeej 58:38a366236bda 471 {
Jeej 58:38a366236bda 472 broadcast = true;
Jeej 58:38a366236bda 473 }
Jeej 76:fda2e34ff19d 474
Jeej 46:665391110051 475 // Construct interface
Jeej 71:f03727ff0f99 476 d7a_itf_t itf = {
Jeej 58:38a366236bda 477 // Dash7 interface
Jeej 58:38a366236bda 478 .type = 0xD7,
Jeej 58:38a366236bda 479 // Switch response type if broadcast
Jeej 58:38a366236bda 480 .cfg.qos.bf.resp = (broadcast)? D7A_RESP_ALL : D7A_RESP_ANY,
Jeej 46:665391110051 481 .cfg.qos.bf.retry = retry,
Jeej 46:665391110051 482 .cfg.qos.bf.record = 0,
Jeej 46:665391110051 483 .cfg.qos.bf.stop_on_err = 0,
Jeej 76:fda2e34ff19d 484 .cfg.to.byte = 0,
Jeej 74:30180585b09e 485 .cfg.te.byte = 0,
Jeej 46:665391110051 486 };
Jeej 46:665391110051 487 memcpy(&itf.cfg.addressee, addressee, sizeof(d7a_addressee_t));
Jeej 58:38a366236bda 488
Jeej 58:38a366236bda 489 // Forward action
Jeej 85:197f67940c0d 490 return d7a_alp_forward_action(p, &itf, true);
Jeej 45:b85384e7d825 491 }
Jeej 45:b85384e7d825 492
Jeej 85:197f67940c0d 493 return 0;
Jeej 76:fda2e34ff19d 494 }
Jeej 76:fda2e34ff19d 495
Jeej 85:197f67940c0d 496 d7a_msg_t** d7a_alp_init_ret(void)
Jeej 76:fda2e34ff19d 497 {
Jeej 85:197f67940c0d 498 d7a_msg_t** ret = (d7a_msg_t**)MALLOC(sizeof(d7a_msg_t*) * (MAX_RESPONSES + 1));
Jeej 85:197f67940c0d 499 for (uint32_t i = 0; i < (MAX_RESPONSES + 1); i++)
Jeej 76:fda2e34ff19d 500 {
Jeej 76:fda2e34ff19d 501 ret[i] = NULL;
Jeej 76:fda2e34ff19d 502 }
Jeej 76:fda2e34ff19d 503
Jeej 76:fda2e34ff19d 504 return ret;
Jeej 76:fda2e34ff19d 505 }
Jeej 76:fda2e34ff19d 506
Jeej 76:fda2e34ff19d 507 d7a_msg_t** d7a_alp_write_file(const uint8_t file_id, const uint32_t offset, const uint32_t size, const uint8_t* const buf, const uint8_t* root_key, d7a_addressee_t* addressee, uint8_t retry, bool resp)
Jeej 76:fda2e34ff19d 508 {
Jeej 83:f4054d0b29ba 509 ALP_FPRINT("\r\n");
Jeej 76:fda2e34ff19d 510
Jeej 76:fda2e34ff19d 511 // Get command buffer
Jeej 77:8c792719a1fc 512 uint8_t* p = &g_alp_buffer[0];
Jeej 76:fda2e34ff19d 513 // Save initial position of the command buffer
Jeej 76:fda2e34ff19d 514 uint8_t* t = p;
Jeej 76:fda2e34ff19d 515
Jeej 76:fda2e34ff19d 516 uint8_t current_tag;
Jeej 76:fda2e34ff19d 517 d7a_msg_t** ret = NULL;
Jeej 76:fda2e34ff19d 518
Jeej 76:fda2e34ff19d 519 // malloc and init pointer array
Jeej 85:197f67940c0d 520 ret = d7a_alp_init_ret();
Jeej 76:fda2e34ff19d 521
Jeej 76:fda2e34ff19d 522 // Tag action
Jeej 76:fda2e34ff19d 523 p += d7a_alp_tag(p, true);
Jeej 76:fda2e34ff19d 524
Jeej 86:420918edb079 525 // Eventual forward
Jeej 86:420918edb079 526 p += d7a_alp_construct_itf(p, addressee, retry, resp);
Jeej 86:420918edb079 527
Jeej 76:fda2e34ff19d 528 // get tag
Jeej 77:8c792719a1fc 529 current_tag = g_alp_tag;
Jeej 76:fda2e34ff19d 530
Jeej 67:9ac9d109b80a 531 // Ask for root permissions
Jeej 67:9ac9d109b80a 532 if (root_key)
Jeej 67:9ac9d109b80a 533 {
Jeej 67:9ac9d109b80a 534 uint8_t req[100];
Jeej 67:9ac9d109b80a 535 uint8_t req_size = d7a_alp_write_action(req, file_id, offset, size, buf, resp);
Jeej 67:9ac9d109b80a 536 p += d7a_alp_perm_request_action(p, req, req_size, root_key, false);
Jeej 67:9ac9d109b80a 537 }
Jeej 76:fda2e34ff19d 538
Jeej 45:b85384e7d825 539 // Write action
Jeej 45:b85384e7d825 540 p += d7a_alp_write_action(p, file_id, offset, size, buf, resp);
Jeej 45:b85384e7d825 541
Jeej 58:38a366236bda 542 // Send command
Jeej 77:8c792719a1fc 543 d7a_com_dump(&g_alp_buffer[0], (uint8_t)(p - t), KAL_COM_FLOW_AT_CMD);
Jeej 59:b42eae56b51b 544
Jeej 58:38a366236bda 545 // Parse responses
Jeej 85:197f67940c0d 546 d7a_alp_construct_resp(ret, current_tag);
Jeej 45:b85384e7d825 547
Jeej 58:38a366236bda 548 return ret;
Jeej 45:b85384e7d825 549 }
Jeej 45:b85384e7d825 550
Jeej 67:9ac9d109b80a 551 d7a_msg_t** d7a_alp_read_file(const uint8_t file_id, const uint32_t offset, const uint32_t size, const uint8_t* root_key, d7a_addressee_t* addressee, uint8_t retry)
Jeej 45:b85384e7d825 552 {
Jeej 83:f4054d0b29ba 553 ALP_FPRINT("\r\n");
Jeej 67:9ac9d109b80a 554
Jeej 58:38a366236bda 555 // Get command buffer
Jeej 77:8c792719a1fc 556 uint8_t* p = &g_alp_buffer[0];
Jeej 58:38a366236bda 557 // Save initial position of the command buffer
Jeej 45:b85384e7d825 558 uint8_t* t = p;
Jeej 45:b85384e7d825 559
Jeej 58:38a366236bda 560 uint8_t current_tag;
Jeej 58:38a366236bda 561 d7a_msg_t** ret = NULL;
Jeej 58:38a366236bda 562
Jeej 58:38a366236bda 563 // malloc and init pointer array
Jeej 85:197f67940c0d 564 ret = d7a_alp_init_ret();
Jeej 76:fda2e34ff19d 565
Jeej 45:b85384e7d825 566 // Tag action
Jeej 45:b85384e7d825 567 p += d7a_alp_tag(p, true);
Jeej 43:28202405094d 568
Jeej 86:420918edb079 569 // Eventual forward
Jeej 86:420918edb079 570 p += d7a_alp_construct_itf(p, addressee, retry, true);
Jeej 86:420918edb079 571
Jeej 58:38a366236bda 572 // get tag
Jeej 77:8c792719a1fc 573 current_tag = g_alp_tag;
Jeej 76:fda2e34ff19d 574
Jeej 67:9ac9d109b80a 575 // Ask for root permissions
Jeej 67:9ac9d109b80a 576 if (root_key)
Jeej 67:9ac9d109b80a 577 {
Jeej 67:9ac9d109b80a 578 uint8_t req[100];
Jeej 82:5d1b101b6d11 579 uint8_t req_size = d7a_alp_read_action(req, file_id, offset, size, true);
Jeej 67:9ac9d109b80a 580 p += d7a_alp_perm_request_action(p, req, req_size, root_key, false);
Jeej 67:9ac9d109b80a 581 }
Jeej 67:9ac9d109b80a 582
Jeej 43:28202405094d 583 // Read action
Jeej 58:38a366236bda 584 p += d7a_alp_read_action(p, file_id, offset, size, true);
Jeej 45:b85384e7d825 585
Jeej 58:38a366236bda 586 // Send command
Jeej 77:8c792719a1fc 587 d7a_com_dump(&g_alp_buffer[0], (uint8_t)(p - t), KAL_COM_FLOW_AT_CMD);
Jeej 59:b42eae56b51b 588
Jeej 58:38a366236bda 589 // Parse responses
Jeej 85:197f67940c0d 590 d7a_alp_construct_resp(ret, current_tag);
Jeej 45:b85384e7d825 591
Jeej 58:38a366236bda 592 return ret;
Jeej 43:28202405094d 593 }
Jeej 43:28202405094d 594
Jeej 67:9ac9d109b80a 595
Jeej 67:9ac9d109b80a 596 d7a_msg_t** d7a_alp_flush_file(const uint8_t file_id, const uint8_t* root_key, d7a_addressee_t* addressee, uint8_t retry, bool resp)
Jeej 67:9ac9d109b80a 597 {
Jeej 83:f4054d0b29ba 598 ALP_FPRINT("\r\n");
Jeej 67:9ac9d109b80a 599
Jeej 67:9ac9d109b80a 600 // Get command buffer
Jeej 77:8c792719a1fc 601 uint8_t* p = &g_alp_buffer[0];
Jeej 67:9ac9d109b80a 602 // Save initial position of the command buffer
Jeej 67:9ac9d109b80a 603 uint8_t* t = p;
Jeej 67:9ac9d109b80a 604
Jeej 67:9ac9d109b80a 605 uint8_t current_tag;
Jeej 67:9ac9d109b80a 606 d7a_msg_t** ret = NULL;
Jeej 67:9ac9d109b80a 607
Jeej 67:9ac9d109b80a 608 // malloc and init pointer array
Jeej 85:197f67940c0d 609 ret = d7a_alp_init_ret();
Jeej 67:9ac9d109b80a 610
Jeej 67:9ac9d109b80a 611 // Tag action
Jeej 67:9ac9d109b80a 612 p += d7a_alp_tag(p, true);
Jeej 67:9ac9d109b80a 613
Jeej 86:420918edb079 614 // Eventual forward
Jeej 86:420918edb079 615 p += d7a_alp_construct_itf(p, addressee, retry, resp);
Jeej 86:420918edb079 616
Jeej 67:9ac9d109b80a 617 // get tag
Jeej 77:8c792719a1fc 618 current_tag = g_alp_tag;
Jeej 76:fda2e34ff19d 619
Jeej 67:9ac9d109b80a 620 // Ask for root permissions
Jeej 67:9ac9d109b80a 621 if (root_key)
Jeej 67:9ac9d109b80a 622 {
Jeej 67:9ac9d109b80a 623 uint8_t req[100];
Jeej 82:5d1b101b6d11 624 uint8_t req_size = d7a_alp_flush_action(req, file_id, resp);
Jeej 67:9ac9d109b80a 625 p += d7a_alp_perm_request_action(p, req, req_size, root_key, false);
Jeej 67:9ac9d109b80a 626 }
Jeej 67:9ac9d109b80a 627
Jeej 76:fda2e34ff19d 628 // Flush action
Jeej 67:9ac9d109b80a 629 p += d7a_alp_flush_action(p, file_id, resp);
Jeej 67:9ac9d109b80a 630
Jeej 67:9ac9d109b80a 631 // Send command
Jeej 77:8c792719a1fc 632 d7a_com_dump(&g_alp_buffer[0], (uint8_t)(p - t), KAL_COM_FLOW_AT_CMD);
Jeej 67:9ac9d109b80a 633
Jeej 67:9ac9d109b80a 634 // Parse responses
Jeej 85:197f67940c0d 635 d7a_alp_construct_resp(ret, current_tag);
Jeej 67:9ac9d109b80a 636
Jeej 67:9ac9d109b80a 637 return ret;
Jeej 67:9ac9d109b80a 638 }
Jeej 67:9ac9d109b80a 639
Jeej 87:9b885d12dbb4 640 void d7a_alp_set_root_key_size(uint8_t size)
Jeej 87:9b885d12dbb4 641 {
Jeej 87:9b885d12dbb4 642 g_alp_root_key_size = size;
Jeej 87:9b885d12dbb4 643 }
Jeej 67:9ac9d109b80a 644
Jeej 76:fda2e34ff19d 645 void d7a_alp_thread()
Jeej 43:28202405094d 646 {
Jeej 83:f4054d0b29ba 647 ALP_FPRINT("(id:0x%08x)\r\n", osThreadGetId());
Jeej 43:28202405094d 648 d7a_com_rx_msg_t* pkt;
Jeej 43:28202405094d 649
Jeej 43:28202405094d 650 while (true)
Jeej 43:28202405094d 651 {
Jeej 43:28202405094d 652 pkt = d7a_alp_wait_pkt();
Jeej 43:28202405094d 653 ASSERT(pkt != NULL, "ALP NULL pkt\r\n");
Jeej 43:28202405094d 654
Jeej 43:28202405094d 655 switch(pkt->id)
Jeej 43:28202405094d 656 {
Jeej 43:28202405094d 657 case KAL_COM_FLOW_AT_RESP:
Jeej 79:82b01c1a62f6 658 ALP_DPRINT("KAL_COM_FLOW_AT_RESP\r\n");
Jeej 45:b85384e7d825 659
Jeej 45:b85384e7d825 660 d7a_alp_new_pl(pkt);
Jeej 43:28202405094d 661
Jeej 43:28202405094d 662 break;
Jeej 59:b42eae56b51b 663 case KAL_COM_FLOW_AT_UNS:
Jeej 79:82b01c1a62f6 664 ALP_DPRINT("KAL_COM_FLOW_AT_UNS\r\n");
Jeej 77:8c792719a1fc 665 if (g_alp_uns_msg)
Jeej 65:ac3844adfe49 666 {
Jeej 65:ac3844adfe49 667 d7a_msg_t** uns = (d7a_msg_t**)MALLOC(sizeof(d7a_msg_t*) * 2);
Jeej 65:ac3844adfe49 668
Jeej 65:ac3844adfe49 669 d7a_alp_rsp_t* pl = d7a_alp_parse_pl(pkt);
Jeej 65:ac3844adfe49 670
Jeej 65:ac3844adfe49 671 uns[0] = pl->msg;
Jeej 65:ac3844adfe49 672 uns[1] = NULL;
Jeej 65:ac3844adfe49 673
Jeej 65:ac3844adfe49 674 FREE(pl);
Jeej 65:ac3844adfe49 675
Jeej 65:ac3844adfe49 676 // Callback
Jeej 77:8c792719a1fc 677 g_alp_uns_msg(uns);
Jeej 65:ac3844adfe49 678 }
Jeej 65:ac3844adfe49 679
Jeej 59:b42eae56b51b 680 FREE(pkt);
Jeej 59:b42eae56b51b 681 break;
Jeej 43:28202405094d 682 default:
Jeej 43:28202405094d 683 EPRINT("ALP Unknown Flow ID 0x%02X\r\n", pkt->id);
Jeej 45:b85384e7d825 684 FREE(pkt);
Jeej 43:28202405094d 685 break;
Jeej 43:28202405094d 686 }
Jeej 43:28202405094d 687 }
Jeej 43:28202405094d 688 }