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

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

MIMEプロパティの変更
  • Property svn:eol-style set to native
  • Property svn:keywords set to Id
  • Property svn:mime-type set to text/x-chdr; charset=SHIFT_JIS
File size: 10.2 KB
RevLine 
[101]1/*
2 * TOPPERS ECHONET Lite Communication Middleware
3 *
4 * Copyright (C) 2014 Cores Co., Ltd. Japan
5 *
6 * 上記著作権者は，以下の(1)〜(4)の条件を満たす場合に限り，本ソフトウェ
7 * ア（本ソフトウェアを改変したものを含む．以下同じ）を使用・複製・改
8 * 変・再配布（以下，利用と呼ぶ）することを無償で許諾する．
9 * (1) 本ソフトウェアをソースコードの形で利用する場合には，上記の著作
10 * 権表示，この利用条件および下記の無保証規定が，そのままの形でソー
11 * スコード中に含まれていること．
12 * (2) 本ソフトウェアを，ライブラリ形式など，他のソフトウェア開発に使
13 * 用できる形で再配布する場合には，再配布に伴うドキュメント（利用
14 * 者マニュアルなど）に，上記の著作権表示，この利用条件および下記
15 * の無保証規定を掲載すること．
16 * (3) 本ソフトウェアを，機器に組み込むなど，他のソフトウェア開発に使
17 * 用できない形で再配布する場合には，次のいずれかの条件を満たすこ
18 * と．
19 * (a) 再配布に伴うドキュメント（利用者マニュアルなど）に，上記の著
20 * 作権表示，この利用条件および下記の無保証規定を掲載すること．
21 * (b) 再配布の形態を，別に定める方法によって，TOPPERSプロジェクトに
22 * 報告すること．
23 * (4) 本ソフトウェアの利用により直接的または間接的に生じるいかなる損
24 * 害からも，上記著作権者およびTOPPERSプロジェクトを免責すること．
25 * また，本ソフトウェアのユーザまたはエンドユーザからのいかなる理
26 * 由に基づく請求からも，上記著作権者およびTOPPERSプロジェクトを
27 * 免責すること．
28 *
29 * 本ソフトウェアは，無保証で提供されているものである．上記著作権者お
30 * よびTOPPERSプロジェクトは，本ソフトウェアに関して，特定の使用目的
31 * に対する適合性も含めて，いかなる保証も行わない．また，本ソフトウェ
32 * アの利用により直接的または間接的に生じたいかなる損害に関しても，そ
33 * の責任を負わない．
34 *
35 * @(#) $Id: echonet_fbs.c 108 2015-06-11 09:15:46Z coas-nagasima $
36 */
37
38/*
39 * 固定長バッファ可変長ストリーム
40 */
41
42#include <kernel.h>
43#include <string.h>
44#include <t_syslog.h>
45#include "echonet_fbs.h"
46
47typedef struct memblk
48{
49 struct memblk *next;
50} memblk_t;
51
52typedef struct mempool
53{
54 int pos;
55 memblk_t *first;
56 uint8_t blockes[NUM_ECHONET_MEMPOOL_BLOCK][ECHONET_MEMPOOL_BLOCK_SIZE];
57} mempool_t;
58
59mempool_t _ECHONET_MEMPOOL;
60mempool_t *ecn_mempoolid = &_ECHONET_MEMPOOL;
61
62#define get_mpf pget_mpf
63
64ER pget_mpf(mempool_t *mpfid, void **buf)
65{
66 ER ret = E_OK;
67
68 loc_cpu();
69
70 if (mpfid->pos < NUM_ECHONET_MEMPOOL_BLOCK) {
71 *buf = mpfid->blockes[mpfid->pos++];
72 }
73 else if (mpfid->first == NULL) {
74 *buf = NULL;
75 ret = E_TMOUT;
76 }
77 else{
78 *buf = mpfid->first;
79 mpfid->first = mpfid->first->next;
80 }
81
82 unl_cpu();
83
84 return ret;
85}
86
87ER rel_mpf(mempool_t *mpfid, void *buf)
88{
89 ER ret = E_OK;
90
91 if ((buf < mpfid->blockes[0]) || (buf >= mpfid->blockes[NUM_ECHONET_MEMPOOL_BLOCK]))
92 return E_PAR;
93
94 if ((((intptr_t)buf - (intptr_t)mpfid->blockes[0]) % ECHONET_MEMPOOL_BLOCK_SIZE) != 0)
95 return E_PAR;
96
97 loc_cpu();
98#ifdef _DEBUG
99 {
100 memblk_t *next;
101 for (next = mpfid->first; next != NULL; next = next->next){
102 if (next == buf)
103 assert(0);
104 }
105 }
106#endif
107 if (mpfid->first == NULL) {
108 mpfid->first = (memblk_t *)buf;
109 mpfid->first->next = NULL;
110 }
111 else {
112 memblk_t *next = mpfid->first;
113 mpfid->first = (memblk_t *)buf;
114 mpfid->first->next = next;
115 }
116
117 unl_cpu();
118
119 return E_OK;
120}
121
122ER get_message(T_ECN_FST_BLK **ppk_msg, int size)
123{
124 void * blk;
125 ER ret;
126
127 *ppk_msg = NULL;
128
129 if ((size <= 0) || (size > DEF_ECN_FBS_BUF_MAXLEN))
130 return E_PAR;
131
132 ret = get_mpf(ecn_mempoolid, &blk);
133 if (ret != E_OK)
134 return ret;
135
136 memset(blk, 0, ECHONET_MEMPOOL_BLOCK_SIZE);
137 *ppk_msg = (T_ECN_FST_BLK *)(blk);
138 (*ppk_msg)->hdr.length = (uint16_t)size;
139
140 return E_OK;
141}
142
143ER iget_message(T_ECN_FST_BLK **ppk_msg, int size)
144{
145 void * blk;
146 ER ret;
147
148 *ppk_msg = NULL;
149
150 if ((size <= 0) || (size > DEF_ECN_FBS_BUF_MAXLEN))
151 return E_PAR;
152
153 ret = pget_mpf(ecn_mempoolid, &blk);
154 if (ret != E_OK)
155 return ret;
156
157 memset(blk, 0, ECHONET_MEMPOOL_BLOCK_SIZE);
158 *ppk_msg = (T_ECN_FST_BLK *)(blk);
159 (*ppk_msg)->hdr.length = (uint16_t)size;
160
161 return E_OK;
162}
163
164void * get_block(T_ECN_FST_BLK *pk_msg, int pos, bool_t exp, int *size)
165{
166 ER ret;
167 void * buf;
168 void * blk;
169 int no;
170 T_ECN_SUB_BLK *mblk;
171 int temp;
172
173 if ((pos < 0) || (!exp && (pos >= pk_msg->hdr.length))) {
174 return NULL;
175 }
176
177 if (pos < DEF_ECN_FBS_FST_DAT_LEN) {
178 buf = (void *)&pk_msg->payload[pos];
179 temp = DEF_ECN_FBS_FST_DAT_LEN - pos;
180 if (temp < *size) {
181 *size = temp;
182 }
183 }
184 else {
185 pos -= DEF_ECN_FBS_FST_DAT_LEN;
186 no = pos / DEF_ECN_FBS_SUB_BUF_LEN;
187
188 mblk = pk_msg->lnk.p_sub[no];
189
190 pos -= no * DEF_ECN_FBS_SUB_BUF_LEN;
191 temp = DEF_ECN_FBS_SUB_BUF_LEN - pos;
192 if (temp < *size) {
193 *size = temp;
194 }
195 if (exp) {
196 if (mblk == NULL) {
197 ret = get_mpf(ecn_mempoolid, &blk);
198 if (ret != E_OK)
199 return NULL;
200
201 mblk = (T_ECN_SUB_BLK *)(blk);
202
203 pk_msg->lnk.p_sub[no] = mblk;
204 }
205
206 temp = pos + *size + no * DEF_ECN_FBS_SUB_BUF_LEN + DEF_ECN_FBS_FST_DAT_LEN;
207 if (pk_msg->hdr.length < temp) {
208 pk_msg->hdr.length = (uint16_t)temp;
209 }
210 }
211
212 buf = &mblk->payload[pos];
213 }
214
215 return buf;
216}
217
218ER release_message(T_ECN_FST_BLK *pk_msg)
219{
220 ER ret;
221 int i;
222
223 for (i = 0; i < 32; i++) {
224 void * blk = pk_msg->lnk.p_sub[i];
225 if (blk == NULL)
226 continue;
227
228 ret = rel_mpf(ecn_mempoolid, blk);
229 if (ret != E_OK)
230 return ret;
231 }
232
233 return rel_mpf(ecn_mempoolid, pk_msg);
234}
235
236int read_message(T_ECN_FST_BLK *pk_msg, int pos, void * dst, int size)
237{
238 int len, rest = size;
239 void * buf;
240
241 if (size <= 0)
242 return 0;
243
244 len = rest;
245 buf = get_block(pk_msg, pos, false, &len);
246 if ((buf == NULL) || (len <= 0))
247 return 0;
248
249 if((pos + len) > pk_msg->hdr.length)
250 len = pk_msg->hdr.length - pos;
251
252 for (; ; ) {
253 memcpy(dst, buf, len);
254 dst = (void *)((intptr_t)dst + len);
255 rest -= len;
256 pos += len;
257
258 if (rest <= 0)
259 break;
260
261 len = rest;
262 buf = get_block(pk_msg, pos, false, &len);
263 if ((buf == NULL) || (len <= 0))
264 return size - rest;
265 }
266
267 return size;
268}
269
270int write_message(const void * src, T_ECN_FST_BLK *pk_msg, int pos, int size)
271{
272 int len, rest = size;
273 void * buf;
274
275 if (size <= 0)
276 return 0;
277
278 len = rest;
279 buf = get_block(pk_msg, pos, true, &len);
280 if ((buf == NULL) || (len <= 0))
281 return 0;
282
283 for (; ; ) {
284 memcpy(buf, src, len);
285 src = (void *)((intptr_t)src +len);
286 rest -= len;
287 pos += len;
288
289 if (rest <= 0)
290 break;
291
292 len = rest;
293 buf = get_block(pk_msg, pos, true, &len);
294 if ((buf == NULL) || (len <= 0))
295 return size - rest;
296 }
297
298 if (pk_msg->hdr.length < pos)
299 pk_msg->hdr.length = (uint16_t)pos;
300
301 return size;
302}
303
304int copy_message(T_ECN_FST_BLK *pk_src, int spos, T_ECN_FST_BLK *pk_dst, int dpos, int size)
305{
306 int dlen, slen, len, rest = size;
307 void * dbuf, *sbuf;
308
309 if (size <= 0)
310 return 0;
311
312 dlen = rest;
313 dbuf = get_block(pk_dst, dpos, true, &dlen);
314 if (dbuf == NULL)
315 return 0;
316
317 slen = rest;
318 sbuf = get_block(pk_src, spos, false, &slen);
319 if (sbuf == NULL)
320 return 0;
321
322 for (; ; ) {
323 len = (dlen < slen) ? dlen : slen;
324
325 if (len == 0)
326 return size - rest;
327
328 memcpy(dbuf, sbuf, len);
329
330 dpos += len;
331 spos += len;
332 rest -= len;
333
334 if (rest <= 0)
335 break;
336
337 dlen = rest;
338 dbuf = get_block(pk_dst, dpos, true, &dlen);
339 if (dbuf == NULL)
340 return size - rest;
341
342 slen = rest;
343 sbuf = get_block(pk_src, spos, false, &slen);
344 if (sbuf == NULL)
345 return size - rest;
346 }
347
348 return size;
349}
350
351/* メモリブロック取得 ok:ポインタ NG:NULL */
352void *_ecn_fbs_mbx_get(ECN_FBS_SIZE_T fa_req_size)
353{
354 T_ECN_FST_BLK *result = NULL;
355
356 get_message(&result, fa_req_size);
357
358 return result;
359}
360
361/* メモリブロック解放 */
362ER _ecn_fbs_mbx_rel(void *p)
363{
364 return release_message((T_ECN_FST_BLK *)p);
365}
366
367/* 領域確保 */
368ER _ecn_fbs_cre(ECN_FBS_SIZE_T fa_req_size, ECN_FBS_ID *fp_id)
369{
370 return get_message(&fp_id->ptr, fa_req_size);
371}
372
373/* 領域解放 */
374ER _ecn_fbs_del(ECN_FBS_ID fa_id)
375{
376 return release_message(fa_id.ptr);
377}
378
379/* 保持データの有無 */
380bool_t _ecn_fbs_exist_data(ECN_FBS_ID fa_id)
381{
382 return fa_id.ptr->hdr.length > fa_id.ptr->hdr.wr;
383}
384
385/* 保持データ長の取得 */
386ECN_FBS_SSIZE_T _ecn_fbs_get_datalen(ECN_FBS_ID fa_id)
387{
388 return fa_id.ptr->hdr.length;
389}
390
391/* 読み取りカーソルの位置取得 */
392ECN_FBS_SSIZE_T _ecn_fbs_get_rpos(ECN_FBS_ID fa_id)
393{
394 return fa_id.ptr->hdr.rd;
395}
396
397/* 読み取りカーソルの位置設定 */
398ER _ecn_fbs_set_rpos(ECN_FBS_ID fa_id, ECN_FBS_SSIZE_T fa_pos)
399{
400 if (fa_id.ptr->hdr.length <= (unsigned int)fa_pos) /* 位置指定が大きすぎる */
401 return E_PAR;
402
403 fa_id.ptr->hdr.rd = fa_pos;
404
405 return E_OK;
406}
407
408/* 読み取りカーソルの位置移動 */
409ER _ecn_fbs_seek_rpos(ECN_FBS_ID fa_id, ECN_FBS_SSIZE_T fa_seek)
410{
411 fa_id.ptr->hdr.rd += fa_seek;
412 if (fa_id.ptr->hdr.rd > fa_id.ptr->hdr.length)
413 fa_id.ptr->hdr.rd = fa_id.ptr->hdr.length;
414
415 return E_OK;
416}
417
418/* 任意指定位置の1byte読み取り */
419int _ecn_fbs_peek(ECN_FBS_ID fa_id, ECN_FBS_SSIZE_T fa_seek)
420{
421 uint8_t result = 0;
422 int ret;
423
424 ret = read_message(fa_id.ptr, fa_seek, &result, 1);
425
426 return (ret == 1) ? result : -1;
427}
428
429/* 任意指定位置の1byte書き込み */
430ER _ecn_fbs_poke(ECN_FBS_ID fa_id, ECN_FBS_SSIZE_T fa_seek, int fa_val)
431{
432 uint8_t result = fa_val;
433 int ret;
434
435 ret = write_message(&result, fa_id.ptr, fa_seek, 1);
436
437 return (ret == 1) ? E_OK : E_PAR;
438}
439
440/* データ追加 */
441ER _ecn_fbs_add_data(ECN_FBS_ID fa_id, const void *fa_dat, ECN_FBS_SSIZE_T fa_len)
442{
443 if((fa_id.ptr->hdr.wr + fa_len) > fa_id.ptr->hdr.length)
444 return E_PAR;
445
446 fa_id.ptr->hdr.wr += write_message(fa_dat, fa_id.ptr, fa_id.ptr->hdr.wr, fa_len);
447 return E_OK;
448}
449
450/* データ追加(領域を自動的に拡張する) */
451ER _ecn_fbs_add_data_ex(ECN_FBS_ID fa_id, const void *fa_dat, ECN_FBS_SSIZE_T fa_len)
452{
453 fa_id.ptr->hdr.wr += write_message(fa_dat, fa_id.ptr, fa_id.ptr->hdr.wr, fa_len);
454 return E_OK;
455}
456
457/* データ取得 */
458ER _ecn_fbs_get_data(ECN_FBS_ID fa_id, void *fa_buf, ECN_FBS_SSIZE_T fa_maxlen, ECN_FBS_SSIZE_T *p_len)
459{
460 int ret;
461
462 ret = read_message(fa_id.ptr, fa_id.ptr->hdr.rd, fa_buf, fa_maxlen);
463 fa_id.ptr->hdr.rd += ret;
464 *p_len = ret;
465
466 return E_OK;
467}
468
469ER ecn_fbs_enqueue(T_ECN_FBS_QUEUE *pk_queue, T_ECN_FST_BLK *pk_buf)
470{
471 loc_cpu();
472
473 *((T_ECN_FST_BLK **)pk_buf->_msg) = NULL;
474 if (pk_queue->pk_head != NULL) {
475 *((T_ECN_FST_BLK **)pk_queue->pk_last->_msg) = pk_buf;
476 }
477 else {
478 pk_queue->pk_head = pk_buf;
479 }
480 pk_queue->pk_last = pk_buf;
481
482 unl_cpu();
483
484 return E_OK;
485}
486
487ER ecn_fbs_dequeue(T_ECN_FBS_QUEUE *pk_queue, T_ECN_FST_BLK **ppk_buf)
488{
489 ER ercd;
490
491 loc_cpu();
492
493 if (pk_queue->pk_head != NULL) {
494 *ppk_buf = pk_queue->pk_head;
495 pk_queue->pk_head = *((T_ECN_FST_BLK **)(*ppk_buf)->_msg);
496 ercd = E_OK;
497 }
498 else {
499 ercd = E_TMOUT;
500 }
501
502 unl_cpu();
503
504 return ercd;
505}
Note: See TracBrowser for help on using the repository browser.