source: uKadecot/trunk/ecnl_ssp/echonet_fbs.c@ 101

Last change on this file since 101 was 101, checked in by coas-nagasima, 8 years ago

TOPPERS/uKadecotのソースコードを追加

  • Property svn:eol-style set to native
  • Property svn:keywords set to Id
  • Property svn:mime-type set to text/plain
File size: 10.2 KB
Line 
1/*
2 * TOPPERS ECHONET Lite Communication Middleware
3 *
4 * Copyright (C) 2014 Cores Co., Ltd. Japan
5 *
6 * ã‹L’˜ìŒ ŽÒ‚́CˆÈ‰º‚Ì(1)`(4)‚ÌðŒ‚ð–ž‚½‚·ê‡‚ÉŒÀ‚èC–{ƒ\ƒtƒgƒEƒF
7 * ƒAi–{ƒ\ƒtƒgƒEƒFƒA‚ð‰ü•Ï‚µ‚½‚à‚Ì‚ðŠÜ‚ށDˆÈ‰º“¯‚¶j‚ðŽg—pE•¡»E‰ü
8 * •ÏEÄ”z•ziˆÈ‰ºC—˜—p‚ƌĂԁj‚·‚邱‚Ƃ𖳏ž‚Å‹–‘ø‚·‚éD
9 * (1) –{ƒ\ƒtƒgƒEƒFƒA‚ðƒ\[ƒXƒR[ƒh‚ÌŒ`‚Å—˜—p‚·‚éê‡‚ɂ́Cã‹L‚Ì’˜ì
10 * Œ •\Ž¦C‚±‚Ì—˜—pðŒ‚¨‚æ‚щº‹L‚Ì–³•ÛØ‹K’肪C‚»‚Ì‚Ü‚Ü‚ÌŒ`‚Ń\[
11 * ƒXƒR[ƒh’†‚ÉŠÜ‚Ü‚ê‚Ä‚¢‚邱‚ƁD
12 * (2) –{ƒ\ƒtƒgƒEƒFƒA‚ðCƒ‰ƒCƒuƒ‰ƒŠŒ`Ž®‚ȂǁC‘¼‚̃\ƒtƒgƒEƒFƒAŠJ”­‚ÉŽg
13 * —p‚Å‚«‚éŒ`‚ōĔz•z‚·‚éê‡‚ɂ́CÄ”z•z‚É”º‚¤ƒhƒLƒ…
14ƒƒ“ƒgi—˜—p
15 * ŽÒƒ}ƒjƒ…
16ƒAƒ‹‚Ȃǁj‚ɁCã‹L‚Ì’˜ìŒ •\Ž¦C‚±‚Ì—˜—pðŒ‚¨‚æ‚щº‹L
17 * ‚Ì–³•ÛØ‹K’è‚ðŒfÚ‚·‚邱‚ƁD
18 * (3) –{ƒ\ƒtƒgƒEƒFƒA‚ðC‹@Ší‚É‘g‚ݍž‚ނȂǁC‘¼‚̃\ƒtƒgƒEƒFƒAŠJ”­‚ÉŽg
19 * —p‚Å‚«‚È‚¢Œ`‚ōĔz•z‚·‚éê‡‚ɂ́CŽŸ‚Ì‚¢‚¸‚ê‚©‚ÌðŒ‚ð–ž‚½‚·‚±
20 * ‚ƁD
21 * (a) Ä”z•z‚É”º‚¤ƒhƒLƒ…
22ƒƒ“ƒgi—˜—pŽÒƒ}ƒjƒ…
23ƒAƒ‹‚Ȃǁj‚ɁCã‹L‚Ì’˜
24 * ìŒ •\Ž¦C‚±‚Ì—˜—pðŒ‚¨‚æ‚щº‹L‚Ì–³•ÛØ‹K’è‚ðŒfÚ‚·‚邱‚ƁD
25 * (b) Ä”z•z‚ÌŒ`‘Ô‚ðC•Ê‚É’è‚ß‚é•û–@‚É‚æ‚Á‚āCTOPPERSƒvƒƒWƒFƒNƒg‚É
26 * •ñ‚·‚邱‚ƁD
27 * (4) –{ƒ\ƒtƒgƒEƒFƒA‚Ì—˜—p‚É‚æ‚è’¼Ú“I‚Ü‚½‚͊ԐړI‚ɐ¶‚¶‚é‚¢‚©‚Ȃ鑹
28 * ŠQ‚©‚ç‚àCã‹L’˜ìŒ ŽÒ‚¨‚æ‚ÑTOPPERSƒvƒƒWƒFƒNƒg‚ð–Ɛӂ·‚邱‚ƁD
29 * ‚Ü‚½C–{ƒ\ƒtƒgƒEƒFƒA‚̃†[ƒU‚Ü‚½‚̓Gƒ“ƒhƒ†[ƒU‚©‚ç‚Ì‚¢‚©‚Ȃ闝
30 * —R‚ÉŠî‚­¿‹‚©‚ç‚àCã‹L’˜ìŒ ŽÒ‚¨‚æ‚ÑTOPPERSƒvƒƒWƒFƒNƒg‚ð
31 * –Ɛӂ·‚邱‚ƁD
32 *
33 * –{ƒ\ƒtƒgƒEƒFƒA‚́C–³•ÛØ‚Å’ñ‹Ÿ‚³‚ê‚Ä‚¢‚é‚à‚Ì‚Å‚ ‚éDã‹L’˜ìŒ ŽÒ‚¨
34 * ‚æ‚ÑTOPPERSƒvƒƒWƒFƒNƒg‚́C–{ƒ\ƒtƒgƒEƒFƒA‚ÉŠÖ‚µ‚āC“Á’è‚ÌŽg—p–Ú“I
35 * ‚ɑ΂·‚é“K‡«‚àŠÜ‚߂āC‚¢‚©‚È‚é•ÛØ‚às‚í‚È‚¢D‚Ü‚½C–{ƒ\ƒtƒgƒEƒF
36 * ƒA‚Ì—˜—p‚É‚æ‚è’¼Ú“I‚Ü‚½‚͊ԐړI‚ɐ¶‚¶‚½‚¢‚©‚Ȃ鑹ŠQ‚ÉŠÖ‚µ‚Ä‚àC‚»
37 * ‚̐ӔC‚𕉂í‚È‚¢D
38 *
39 * @(#) $Id: echonet_fbs.c 101 2015-06-02 15:37:23Z coas-nagasima $
40 */
41
42/*
43 * ŒÅ’è’·ƒoƒbƒtƒ@‰Â•Ï’·ƒXƒgƒŠ[ƒ€
44 */
45
46#include <kernel.h>
47#include <string.h>
48#include <t_syslog.h>
49#include "echonet_fbs.h"
50
51typedef struct memblk
52{
53 struct memblk *next;
54} memblk_t;
55
56typedef struct mempool
57{
58 int pos;
59 memblk_t *first;
60 uint8_t blockes[NUM_ECHONET_MEMPOOL_BLOCK][ECHONET_MEMPOOL_BLOCK_SIZE];
61} mempool_t;
62
63mempool_t _ECHONET_MEMPOOL;
64mempool_t *ecn_mempoolid = &_ECHONET_MEMPOOL;
65
66#define get_mpf pget_mpf
67
68ER pget_mpf(mempool_t *mpfid, void **buf)
69{
70 ER ret = E_OK;
71
72 loc_cpu();
73
74 if (mpfid->pos < NUM_ECHONET_MEMPOOL_BLOCK) {
75 *buf = mpfid->blockes[mpfid->pos++];
76 }
77 else if (mpfid->first == NULL) {
78 *buf = NULL;
79 ret = E_TMOUT;
80 }
81 else{
82 *buf = mpfid->first;
83 mpfid->first = mpfid->first->next;
84 }
85
86 unl_cpu();
87
88 return ret;
89}
90
91ER rel_mpf(mempool_t *mpfid, void *buf)
92{
93 ER ret = E_OK;
94
95 if ((buf < mpfid->blockes[0]) || (buf >= mpfid->blockes[NUM_ECHONET_MEMPOOL_BLOCK]))
96 return E_PAR;
97
98 if ((((intptr_t)buf - (intptr_t)mpfid->blockes[0]) % ECHONET_MEMPOOL_BLOCK_SIZE) != 0)
99 return E_PAR;
100
101 loc_cpu();
102#ifdef _DEBUG
103 {
104 memblk_t *next;
105 for (next = mpfid->first; next != NULL; next = next->next){
106 if (next == buf)
107 assert(0);
108 }
109 }
110#endif
111 if (mpfid->first == NULL) {
112 mpfid->first = (memblk_t *)buf;
113 mpfid->first->next = NULL;
114 }
115 else {
116 memblk_t *next = mpfid->first;
117 mpfid->first = (memblk_t *)buf;
118 mpfid->first->next = next;
119 }
120
121 unl_cpu();
122
123 return E_OK;
124}
125
126ER get_message(T_ECN_FST_BLK **ppk_msg, int size)
127{
128 void * blk;
129 ER ret;
130
131 *ppk_msg = NULL;
132
133 if ((size <= 0) || (size > DEF_ECN_FBS_BUF_MAXLEN))
134 return E_PAR;
135
136 ret = get_mpf(ecn_mempoolid, &blk);
137 if (ret != E_OK)
138 return ret;
139
140 memset(blk, 0, ECHONET_MEMPOOL_BLOCK_SIZE);
141 *ppk_msg = (T_ECN_FST_BLK *)(blk);
142 (*ppk_msg)->hdr.length = (uint16_t)size;
143
144 return E_OK;
145}
146
147ER iget_message(T_ECN_FST_BLK **ppk_msg, int size)
148{
149 void * blk;
150 ER ret;
151
152 *ppk_msg = NULL;
153
154 if ((size <= 0) || (size > DEF_ECN_FBS_BUF_MAXLEN))
155 return E_PAR;
156
157 ret = pget_mpf(ecn_mempoolid, &blk);
158 if (ret != E_OK)
159 return ret;
160
161 memset(blk, 0, ECHONET_MEMPOOL_BLOCK_SIZE);
162 *ppk_msg = (T_ECN_FST_BLK *)(blk);
163 (*ppk_msg)->hdr.length = (uint16_t)size;
164
165 return E_OK;
166}
167
168void * get_block(T_ECN_FST_BLK *pk_msg, int pos, bool_t exp, int *size)
169{
170 ER ret;
171 void * buf;
172 void * blk;
173 int no;
174 T_ECN_SUB_BLK *mblk;
175 int temp;
176
177 if ((pos < 0) || (!exp && (pos >= pk_msg->hdr.length))) {
178 return NULL;
179 }
180
181 if (pos < DEF_ECN_FBS_FST_DAT_LEN) {
182 buf = (void *)&pk_msg->payload[pos];
183 temp = DEF_ECN_FBS_FST_DAT_LEN - pos;
184 if (temp < *size) {
185 *size = temp;
186 }
187 }
188 else {
189 pos -= DEF_ECN_FBS_FST_DAT_LEN;
190 no = pos / DEF_ECN_FBS_SUB_BUF_LEN;
191
192 mblk = pk_msg->lnk.p_sub[no];
193
194 pos -= no * DEF_ECN_FBS_SUB_BUF_LEN;
195 temp = DEF_ECN_FBS_SUB_BUF_LEN - pos;
196 if (temp < *size) {
197 *size = temp;
198 }
199 if (exp) {
200 if (mblk == NULL) {
201 ret = get_mpf(ecn_mempoolid, &blk);
202 if (ret != E_OK)
203 return NULL;
204
205 mblk = (T_ECN_SUB_BLK *)(blk);
206
207 pk_msg->lnk.p_sub[no] = mblk;
208 }
209
210 temp = pos + *size + no * DEF_ECN_FBS_SUB_BUF_LEN + DEF_ECN_FBS_FST_DAT_LEN;
211 if (pk_msg->hdr.length < temp) {
212 pk_msg->hdr.length = (uint16_t)temp;
213 }
214 }
215
216 buf = &mblk->payload[pos];
217 }
218
219 return buf;
220}
221
222ER release_message(T_ECN_FST_BLK *pk_msg)
223{
224 ER ret;
225 int i;
226
227 for (i = 0; i < 32; i++) {
228 void * blk = pk_msg->lnk.p_sub[i];
229 if (blk == NULL)
230 continue;
231
232 ret = rel_mpf(ecn_mempoolid, blk);
233 if (ret != E_OK)
234 return ret;
235 }
236
237 return rel_mpf(ecn_mempoolid, pk_msg);
238}
239
240int read_message(T_ECN_FST_BLK *pk_msg, int pos, void * dst, int size)
241{
242 int len, rest = size;
243 void * buf;
244
245 if (size <= 0)
246 return 0;
247
248 len = rest;
249 buf = get_block(pk_msg, pos, false, &len);
250 if ((buf == NULL) || (len <= 0))
251 return 0;
252
253 if((pos + len) > pk_msg->hdr.length)
254 len = pk_msg->hdr.length - pos;
255
256 for (; ; ) {
257 memcpy(dst, buf, len);
258 dst = (void *)((intptr_t)dst + len);
259 rest -= len;
260 pos += len;
261
262 if (rest <= 0)
263 break;
264
265 len = rest;
266 buf = get_block(pk_msg, pos, false, &len);
267 if ((buf == NULL) || (len <= 0))
268 return size - rest;
269 }
270
271 return size;
272}
273
274int write_message(const void * src, T_ECN_FST_BLK *pk_msg, int pos, int size)
275{
276 int len, rest = size;
277 void * buf;
278
279 if (size <= 0)
280 return 0;
281
282 len = rest;
283 buf = get_block(pk_msg, pos, true, &len);
284 if ((buf == NULL) || (len <= 0))
285 return 0;
286
287 for (; ; ) {
288 memcpy(buf, src, len);
289 src = (void *)((intptr_t)src +len);
290 rest -= len;
291 pos += len;
292
293 if (rest <= 0)
294 break;
295
296 len = rest;
297 buf = get_block(pk_msg, pos, true, &len);
298 if ((buf == NULL) || (len <= 0))
299 return size - rest;
300 }
301
302 if (pk_msg->hdr.length < pos)
303 pk_msg->hdr.length = (uint16_t)pos;
304
305 return size;
306}
307
308int copy_message(T_ECN_FST_BLK *pk_src, int spos, T_ECN_FST_BLK *pk_dst, int dpos, int size)
309{
310 int dlen, slen, len, rest = size;
311 void * dbuf, *sbuf;
312
313 if (size <= 0)
314 return 0;
315
316 dlen = rest;
317 dbuf = get_block(pk_dst, dpos, true, &dlen);
318 if (dbuf == NULL)
319 return 0;
320
321 slen = rest;
322 sbuf = get_block(pk_src, spos, false, &slen);
323 if (sbuf == NULL)
324 return 0;
325
326 for (; ; ) {
327 len = (dlen < slen) ? dlen : slen;
328
329 if (len == 0)
330 return size - rest;
331
332 memcpy(dbuf, sbuf, len);
333
334 dpos += len;
335 spos += len;
336 rest -= len;
337
338 if (rest <= 0)
339 break;
340
341 dlen = rest;
342 dbuf = get_block(pk_dst, dpos, true, &dlen);
343 if (dbuf == NULL)
344 return size - rest;
345
346 slen = rest;
347 sbuf = get_block(pk_src, spos, false, &slen);
348 if (sbuf == NULL)
349 return size - rest;
350 }
351
352 return size;
353}
354
355/* ƒƒ‚ƒŠƒuƒƒbƒNŽæ“¾ ok:ƒ|ƒCƒ“ƒ^ NG:NULL */
356void *_ecn_fbs_mbx_get(ECN_FBS_SIZE_T fa_req_size)
357{
358 T_ECN_FST_BLK *result = NULL;
359
360 get_message(&result, fa_req_size);
361
362 return result;
363}
364
365/* ƒƒ‚ƒŠƒuƒƒbƒN‰ð•ú */
366ER _ecn_fbs_mbx_rel(void *p)
367{
368 return release_message((T_ECN_FST_BLK *)p);
369}
370
371/* —̈æŠm•Û */
372ER _ecn_fbs_cre(ECN_FBS_SIZE_T fa_req_size, ECN_FBS_ID *fp_id)
373{
374 return get_message(&fp_id->ptr, fa_req_size);
375}
376
377/* —̈æ‰ð•ú */
378ER _ecn_fbs_del(ECN_FBS_ID fa_id)
379{
380 return release_message(fa_id.ptr);
381}
382
383/* •ÛŽƒf[ƒ^‚Ì—L–³ */
384bool_t _ecn_fbs_exist_data(ECN_FBS_ID fa_id)
385{
386 return fa_id.ptr->hdr.length > fa_id.ptr->hdr.wr;
387}
388
389/* •ÛŽƒf[ƒ^’·‚̎擾 */
390ECN_FBS_SSIZE_T _ecn_fbs_get_datalen(ECN_FBS_ID fa_id)
391{
392 return fa_id.ptr->hdr.length;
393}
394
395/* “Ç‚ÝŽæ‚èƒJ[ƒ\ƒ‹‚̈ʒuŽæ“¾ */
396ECN_FBS_SSIZE_T _ecn_fbs_get_rpos(ECN_FBS_ID fa_id)
397{
398 return fa_id.ptr->hdr.rd;
399}
400
401/* “Ç‚ÝŽæ‚èƒJ[ƒ\ƒ‹‚̈ʒuÝ’è */
402ER _ecn_fbs_set_rpos(ECN_FBS_ID fa_id, ECN_FBS_SSIZE_T fa_pos)
403{
404 if (fa_id.ptr->hdr.length <= (unsigned int)fa_pos) /* ˆÊ’uŽw’肪‘å‚«‚·‚¬‚é */
405 return E_PAR;
406
407 fa_id.ptr->hdr.rd = fa_pos;
408
409 return E_OK;
410}
411
412/* “Ç‚ÝŽæ‚èƒJ[ƒ\ƒ‹‚̈ʒuˆÚ“® */
413ER _ecn_fbs_seek_rpos(ECN_FBS_ID fa_id, ECN_FBS_SSIZE_T fa_seek)
414{
415 fa_id.ptr->hdr.rd += fa_seek;
416 if (fa_id.ptr->hdr.rd > fa_id.ptr->hdr.length)
417 fa_id.ptr->hdr.rd = fa_id.ptr->hdr.length;
418
419 return E_OK;
420}
421
422/* ”CˆÓŽw’èˆÊ’u‚Ì1byte“Ç‚ÝŽæ‚è */
423int _ecn_fbs_peek(ECN_FBS_ID fa_id, ECN_FBS_SSIZE_T fa_seek)
424{
425 uint8_t result = 0;
426 int ret;
427
428 ret = read_message(fa_id.ptr, fa_seek, &result, 1);
429
430 return (ret == 1) ? result : -1;
431}
432
433/* ”CˆÓŽw’èˆÊ’u‚Ì1byte‘‚«ž‚Ý */
434ER _ecn_fbs_poke(ECN_FBS_ID fa_id, ECN_FBS_SSIZE_T fa_seek, int fa_val)
435{
436 uint8_t result = fa_val;
437 int ret;
438
439 ret = write_message(&result, fa_id.ptr, fa_seek, 1);
440
441 return (ret == 1) ? E_OK : E_PAR;
442}
443
444/* ƒf[ƒ^’ljÁ */
445ER _ecn_fbs_add_data(ECN_FBS_ID fa_id, const void *fa_dat, ECN_FBS_SSIZE_T fa_len)
446{
447 if((fa_id.ptr->hdr.wr + fa_len) > fa_id.ptr->hdr.length)
448 return E_PAR;
449
450 fa_id.ptr->hdr.wr += write_message(fa_dat, fa_id.ptr, fa_id.ptr->hdr.wr, fa_len);
451 return E_OK;
452}
453
454/* ƒf[ƒ^’ljÁ(—̈æ‚ðŽ©“®“I‚ÉŠg’£‚·‚é) */
455ER _ecn_fbs_add_data_ex(ECN_FBS_ID fa_id, const void *fa_dat, ECN_FBS_SSIZE_T fa_len)
456{
457 fa_id.ptr->hdr.wr += write_message(fa_dat, fa_id.ptr, fa_id.ptr->hdr.wr, fa_len);
458 return E_OK;
459}
460
461/* ƒf[ƒ^Žæ“¾ */
462ER _ecn_fbs_get_data(ECN_FBS_ID fa_id, void *fa_buf, ECN_FBS_SSIZE_T fa_maxlen, ECN_FBS_SSIZE_T *p_len)
463{
464 int ret;
465
466 ret = read_message(fa_id.ptr, fa_id.ptr->hdr.rd, fa_buf, fa_maxlen);
467 fa_id.ptr->hdr.rd += ret;
468 *p_len = ret;
469
470 return E_OK;
471}
472
473ER ecn_fbs_enqueue(T_ECN_FBS_QUEUE *pk_queue, T_ECN_FST_BLK *pk_buf)
474{
475 loc_cpu();
476
477 *((T_ECN_FST_BLK **)pk_buf->_msg) = NULL;
478 if (pk_queue->pk_head != NULL) {
479 *((T_ECN_FST_BLK **)pk_queue->pk_last->_msg) = pk_buf;
480 }
481 else {
482 pk_queue->pk_head = pk_buf;
483 }
484 pk_queue->pk_last = pk_buf;
485
486 unl_cpu();
487
488 return E_OK;
489}
490
491ER ecn_fbs_dequeue(T_ECN_FBS_QUEUE *pk_queue, T_ECN_FST_BLK **ppk_buf)
492{
493 ER ercd;
494
495 loc_cpu();
496
497 if (pk_queue->pk_head != NULL) {
498 *ppk_buf = pk_queue->pk_head;
499 pk_queue->pk_head = *((T_ECN_FST_BLK **)(*ppk_buf)->_msg);
500 ercd = E_OK;
501 }
502 else {
503 ercd = E_TMOUT;
504 }
505
506 unl_cpu();
507
508 return ercd;
509}
Note: See TracBrowser for help on using the repository browser.