sub_device_protocol.c 27 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973
  1. /*******************************************************
  2. * File name:sub_device_protocol.c
  3. * Author: LIJIAN
  4. * Versions: 0.1
  5. * Description: APIs for sub device.
  6. * History:
  7. * 1.Date:
  8. * Author:
  9. * Modification:
  10. *********************************************************/
  11. #include "sub_device_protocol.h"
  12. #include "pando_sys.h"
  13. #define DEFAULT_TLV_BLOCK_SIZE 128
  14. /***************** Local types ****************/
  15. struct params_block_indicator
  16. {
  17. uint16_t tlv_count; //the count of tlv in TLVs
  18. uint16_t handled_length;
  19. };
  20. struct property_indicator
  21. {
  22. int position;
  23. };
  24. /**********************************************/
  25. static struct property_indicator s_current_property;
  26. static struct params_block_indicator s_current_param;
  27. struct sub_device_base_params base_params; //Basic param of sub device
  28. static uint16_t current_tlv_block_size = 0; //Current params block size, include count.
  29. static uint16_t tlv_block_buffer_size; //Size of buffer pre-malloced to contain params block.
  30. static uint16_t get_tlv_count(struct TLVs *params_block);
  31. static uint16_t get_tlv_type(struct TLV *params_in);
  32. static uint16_t get_tlv_len(struct TLV *params_in);
  33. static struct TLV * get_tlv_value(struct TLV *params_in, void *value);
  34. static struct TLV *get_tlv_param(struct TLV *params_in, uint16_t *type, uint16_t *length, void *value);
  35. static void FUNCTION_ATTRIBUTE *copy_return_next(void *dst, void *src,
  36. unsigned int src_len)
  37. {
  38. pd_memcpy(dst, src, src_len);
  39. dst = (char *)dst + src_len;
  40. return dst;
  41. }
  42. static struct TLVs FUNCTION_ATTRIBUTE *get_next_property(struct pando_property *next_property, struct pando_property *property_body);
  43. static uint8_t FUNCTION_ATTRIBUTE is_tlv_need_length(uint16_t type);
  44. static uint8_t FUNCTION_ATTRIBUTE get_type_length(uint16_t type);
  45. static struct sub_device_buffer * FUNCTION_ATTRIBUTE create_package(
  46. uint16_t flags, uint16_t payload_type)
  47. {
  48. struct sub_device_buffer *data_buffer = NULL;
  49. struct device_header *header = NULL;
  50. data_buffer = (struct sub_device_buffer *)pd_malloc(sizeof(struct sub_device_buffer));
  51. pd_memset(data_buffer, 0, sizeof(struct sub_device_buffer));
  52. //only create header
  53. data_buffer->buffer_length = DEV_HEADER_LEN;
  54. data_buffer->buffer = (uint8_t *)pd_malloc(data_buffer->buffer_length);
  55. pd_memset(data_buffer->buffer, 0, data_buffer->buffer_length);
  56. header = (struct device_header *)data_buffer->buffer;
  57. if (payload_type == PAYLOAD_TYPE_COMMAND)
  58. {
  59. base_params.command_sequence++;
  60. header->frame_seq = host32_to_net(base_params.command_sequence);
  61. }
  62. else if (payload_type == PAYLOAD_TYPE_EVENT)
  63. {
  64. base_params.event_sequence++;
  65. header->frame_seq = host32_to_net(base_params.event_sequence);
  66. }if (payload_type == PAYLOAD_TYPE_DATA)
  67. {
  68. base_params.data_sequence++;
  69. header->frame_seq = host32_to_net(base_params.data_sequence);
  70. }
  71. header->flags = host16_to_net(flags);
  72. header->magic = MAGIC_HEAD_SUB_DEVICE;
  73. header->payload_type = host16_to_net(payload_type);
  74. return data_buffer;
  75. }
  76. // we must maintain the position of tlv for next get operation
  77. // handled_length means all the tlv has been handled
  78. static void FUNCTION_ATTRIBUTE cal_current_position(uint16_t type, uint16_t len)
  79. {
  80. s_current_param.tlv_count--;
  81. s_current_param.handled_length += len
  82. + sizeof(type) + sizeof(len) * is_tlv_need_length(type);
  83. if (s_current_param.tlv_count == 0)
  84. {
  85. //if is handle data property's params
  86. if (s_current_property.position != 0) //mean has already decode with property
  87. {
  88. s_current_property.position += s_current_param.handled_length;
  89. }
  90. //all tlv param has been handled.
  91. s_current_param.tlv_count= 0;
  92. s_current_param.handled_length= 0;
  93. }
  94. }
  95. static struct TLV FUNCTION_ATTRIBUTE *get_current_tlv(struct TLVs *params)
  96. {
  97. if (s_current_param.handled_length == 0) //first tlv
  98. {
  99. s_current_param.tlv_count = net16_to_host(params->count);
  100. }
  101. return (struct TLV *)((uint8_t *)params + sizeof(struct TLVs)
  102. + s_current_param.handled_length);
  103. }
  104. static void FUNCTION_ATTRIBUTE get_value(struct TLVs *params, void *val)
  105. {
  106. uint16_t type = 0;
  107. uint16_t len = 0;
  108. struct TLV* params_in = get_current_tlv(params);
  109. get_tlv_param(params_in, &type, &len, val);
  110. cal_current_position(type, len);
  111. }
  112. static void FUNCTION_ATTRIBUTE *get_string(struct TLVs *params, uint16_t *len)
  113. {
  114. uint16_t type = 0;
  115. void *val = NULL;
  116. struct TLV* params_in = get_current_tlv(params);
  117. val = params_in->value;
  118. *len = get_tlv_len(params_in);
  119. type = get_tlv_type(params_in);
  120. cal_current_position(type, *len);
  121. return val;
  122. }
  123. int FUNCTION_ATTRIBUTE init_sub_device(struct sub_device_base_params params)
  124. {
  125. base_params.data_sequence = params.data_sequence;
  126. base_params.event_sequence = params.event_sequence;
  127. base_params.command_sequence = params.command_sequence;
  128. return 0;
  129. }
  130. struct TLVs * FUNCTION_ATTRIBUTE create_params_block(void)
  131. {
  132. struct TLVs *tlv_block = NULL;
  133. uint8_t need_length;
  134. current_tlv_block_size = 0;
  135. tlv_block_buffer_size = DEFAULT_TLV_BLOCK_SIZE;
  136. tlv_block = (struct TLVs *)pd_malloc(tlv_block_buffer_size);
  137. if (tlv_block == NULL)
  138. {
  139. return NULL;
  140. }
  141. tlv_block->count = 0; //init tlv number to 0 when create
  142. current_tlv_block_size = sizeof(struct TLVs); //size of block equals sizeof count when no param added.
  143. return tlv_block;
  144. }
  145. int FUNCTION_ATTRIBUTE add_next_param(struct TLVs *params_block, uint16_t next_type, uint16_t next_length, void *next_value)
  146. {
  147. uint16_t type;
  148. uint16_t conver_length;
  149. uint8_t need_length;
  150. uint8_t tlv_length;
  151. uint8_t tmp_value[8];
  152. uint8_t *tlv_position;
  153. struct TLVs *new_property_block = NULL;
  154. uint16_t current_count = net16_to_host(params_block->count); //count has been changed endian when create params block
  155. need_length = is_tlv_need_length(next_type);
  156. if (1 == need_length)
  157. {
  158. }
  159. else if (0 == need_length)
  160. {
  161. if(next_length != get_type_length(next_type))
  162. {
  163. pd_printf("Param type dismatch with the input length\n");
  164. return -1;
  165. }
  166. }
  167. else
  168. {
  169. return -1;
  170. }
  171. //extend buffer when default buffer is not enough to add new param
  172. if (current_tlv_block_size + next_length + sizeof(struct TLV)
  173. - (!need_length) * sizeof(next_length) > tlv_block_buffer_size)
  174. {
  175. tlv_block_buffer_size = tlv_block_buffer_size + DEFAULT_TLV_BLOCK_SIZE + next_length;
  176. new_property_block = (struct TLVs *)pd_malloc(tlv_block_buffer_size);
  177. pd_memcpy(new_property_block, params_block, current_tlv_block_size);
  178. pd_free(params_block);
  179. params_block = new_property_block;
  180. }
  181. current_count++;
  182. tlv_position = (uint8_t *)params_block + current_tlv_block_size;
  183. //change count into net endian
  184. params_block->count = host16_to_net(current_count);
  185. //content of tlv need to be copied, otherwise when variable is not aligned.
  186. type = host16_to_net(next_type);
  187. pd_memcpy(tlv_position, &type, sizeof(type));
  188. tlv_position += sizeof(type);
  189. if (need_length)
  190. {
  191. conver_length = host16_to_net(next_length);
  192. pd_memcpy(tlv_position, &conver_length, sizeof(conver_length));
  193. tlv_position += sizeof(conver_length);
  194. }
  195. switch(next_type)
  196. {
  197. case TLV_TYPE_FLOAT64 :
  198. *((double *)tmp_value) = host64f_to_net(*((uint64_t *)next_value));
  199. pd_memcpy(tlv_position, tmp_value, next_length);
  200. break;
  201. case TLV_TYPE_FLOAT32 :
  202. *((float *)tmp_value) = host32f_to_net(*((float *)next_value));
  203. pd_memcpy(tlv_position, tmp_value, next_length);
  204. break;
  205. case TLV_TYPE_INT16:
  206. *((int16_t *)tmp_value) = host16_to_net(*((int16_t *)next_value));
  207. pd_memcpy(tlv_position, tmp_value, next_length);
  208. break;
  209. case TLV_TYPE_UINT16:
  210. *((uint16_t *)tmp_value) = host16_to_net(*((uint16_t *)next_value));
  211. pd_memcpy(tlv_position, tmp_value, next_length);
  212. break;
  213. case TLV_TYPE_INT32:
  214. *((int32_t *)tmp_value) = host32_to_net(*((int32_t *)next_value));
  215. pd_memcpy(tlv_position, tmp_value, next_length);
  216. break;
  217. case TLV_TYPE_UINT32:
  218. *((uint32_t *)tmp_value) = host32_to_net(*((uint32_t *)next_value));
  219. pd_memcpy(tlv_position, tmp_value, next_length);
  220. break;
  221. case TLV_TYPE_UINT64:
  222. *((uint64_t *)tmp_value) = host64_to_net(*((uint64_t *)next_value));
  223. pd_memcpy(tlv_position, tmp_value, next_length);
  224. break;
  225. case TLV_TYPE_INT64:
  226. *((int64_t *)tmp_value) = host64_to_net(*((int64_t *)next_value));
  227. pd_memcpy(tlv_position, tmp_value, next_length);
  228. break;
  229. default:
  230. pd_memcpy(tlv_position, next_value, next_length);
  231. break;
  232. }
  233. current_tlv_block_size = current_tlv_block_size + next_length
  234. + sizeof(struct TLV) - (!need_length) * sizeof(next_length);
  235. return 0;
  236. }
  237. void FUNCTION_ATTRIBUTE delete_device_package(struct sub_device_buffer *device_buffer)
  238. {
  239. if (device_buffer != NULL)
  240. {
  241. if (device_buffer->buffer != NULL)
  242. {
  243. pd_free(device_buffer->buffer);
  244. device_buffer->buffer = NULL;
  245. }
  246. }
  247. pd_free(device_buffer);
  248. device_buffer = NULL;
  249. }
  250. void FUNCTION_ATTRIBUTE delete_params_block(struct TLVs *params_block)
  251. {
  252. if (params_block != NULL)
  253. {
  254. pd_free(params_block);
  255. params_block = NULL;
  256. }
  257. }
  258. struct sub_device_buffer * FUNCTION_ATTRIBUTE
  259. create_data_package(uint16_t flags)
  260. {
  261. return create_package(flags, PAYLOAD_TYPE_DATA);
  262. }
  263. struct sub_device_buffer * FUNCTION_ATTRIBUTE
  264. create_event_package(uint16_t flags)
  265. {
  266. return create_package(flags, PAYLOAD_TYPE_EVENT);
  267. }
  268. struct sub_device_buffer * FUNCTION_ATTRIBUTE
  269. create_command_package(uint16_t flags)
  270. {
  271. return create_package(flags, PAYLOAD_TYPE_COMMAND);
  272. }
  273. struct TLVs * FUNCTION_ATTRIBUTE get_sub_device_command(
  274. struct sub_device_buffer *device_buffer, struct pando_command *command_body)
  275. {
  276. struct pando_command *tmp_body = (struct pando_command *)(device_buffer->buffer + DEV_HEADER_LEN);
  277. struct device_header *head = (struct device_header *)device_buffer->buffer;
  278. base_params.command_sequence = net32_to_host(head->frame_seq);
  279. command_body->sub_device_id = net16_to_host(tmp_body->sub_device_id);
  280. command_body->command_num = net16_to_host(tmp_body->command_num);
  281. command_body->priority = net16_to_host(tmp_body->priority);
  282. command_body->params->count = net16_to_host(tmp_body->params->count);
  283. return (struct TLVs *)(device_buffer->buffer + DEV_HEADER_LEN
  284. + sizeof(struct pando_command) - sizeof(struct TLVs));
  285. }
  286. uint16_t FUNCTION_ATTRIBUTE
  287. get_tlv_count(struct TLVs *params_block)
  288. {
  289. uint16_t count = 0;
  290. if (params_block == NULL)
  291. {
  292. return -1;
  293. }
  294. pd_memcpy((void*)(&count), (void*)(&(params_block->count)), sizeof(count));
  295. return count;
  296. }
  297. uint16_t FUNCTION_ATTRIBUTE get_tlv_type(struct TLV *params_in)
  298. {
  299. uint16_t type = 0;
  300. pd_memcpy((void*)(&type), (void*)(&(params_in->type)), sizeof(params_in->type));
  301. type = net16_to_host(type);
  302. return type;
  303. }
  304. uint16_t FUNCTION_ATTRIBUTE get_tlv_len(struct TLV *params_in)
  305. {
  306. uint8_t need_length;
  307. uint16_t type = 0;
  308. uint16_t length = 0;
  309. void *value_pos = NULL;
  310. if (params_in == NULL)
  311. {
  312. return -1;
  313. }
  314. pd_memcpy((void*)(&type), (void*)(&(params_in->type)), sizeof(params_in->type));
  315. type = net16_to_host(type);
  316. need_length = is_tlv_need_length(type);
  317. if (need_length == 1)
  318. {
  319. pd_memcpy((void*)(&length), (void*)(&(params_in->length)), sizeof(params_in->length));
  320. length = net16_to_host(length);
  321. }
  322. else
  323. {
  324. length = get_type_length(type);
  325. }
  326. return length;
  327. }
  328. struct TLV * FUNCTION_ATTRIBUTE get_tlv_value(struct TLV *params_in, void *value)
  329. {
  330. uint8_t need_length;
  331. void *value_pos = NULL;
  332. uint16_t type_p;
  333. uint16_t length_p;
  334. uint16_t *type = &type_p;
  335. uint16_t *length = &length_p;
  336. pd_memcpy((void*)type, (void*)(&(params_in->type)), sizeof(params_in->type));
  337. *type = net16_to_host(*type);
  338. need_length = is_tlv_need_length(*type);
  339. if (need_length == 1)
  340. {
  341. pd_memcpy((void*)length, (void*)(&(params_in->length)), sizeof(params_in->length));
  342. *length = net16_to_host(*length);
  343. pd_memcpy((void*)value, (void*)(params_in->value), *length);
  344. }
  345. else
  346. {
  347. *length = get_type_length(*type);
  348. value_pos = (uint8_t *)params_in + sizeof(params_in->type);
  349. pd_memcpy(value, value_pos, *length);
  350. switch (*type)
  351. {
  352. case TLV_TYPE_FLOAT64:
  353. net64f_to_host(*(double *)value);
  354. break;
  355. case TLV_TYPE_UINT64:
  356. case TLV_TYPE_INT64:
  357. net64_to_host(*(uint64_t *)value);
  358. break;
  359. case TLV_TYPE_FLOAT32:
  360. net32f_to_host(*(float *)value);
  361. break;
  362. case TLV_TYPE_UINT32:
  363. case TLV_TYPE_INT32:
  364. net32_to_host(*(uint32_t *)value);
  365. break;
  366. case TLV_TYPE_INT8:
  367. case TLV_TYPE_UINT8:
  368. case TLV_TYPE_BOOL:
  369. break;
  370. case TLV_TYPE_INT16:
  371. case TLV_TYPE_UINT16:
  372. net16_to_host(*(uint16_t *)value);
  373. break;
  374. default:
  375. break;
  376. }//switch
  377. }//need_length == 0
  378. return (struct TLV *)((uint8_t *)params_in + *length + sizeof(struct TLV)
  379. - (!need_length) * sizeof(*length));
  380. }
  381. struct TLV * FUNCTION_ATTRIBUTE get_tlv_param(struct TLV *params_in, uint16_t *type,
  382. uint16_t *length, void *value)
  383. {
  384. uint8_t need_length;
  385. void *value_pos = NULL;
  386. pd_memcpy((void*)type, (void*)(&(params_in->type)), sizeof(params_in->type));
  387. *type = net16_to_host(*type);
  388. need_length = is_tlv_need_length(*type);
  389. if (need_length == 1)
  390. {
  391. pd_memcpy((void*)length, (void*)(&(params_in->length)), sizeof(params_in->length));
  392. *length = net16_to_host(*length);
  393. pd_memcpy((void*)value, (void*)(params_in->value), *length);
  394. }
  395. else
  396. {
  397. *length = get_type_length(*type);
  398. value_pos = (uint8_t *)params_in + sizeof(params_in->type);
  399. pd_memcpy((void*)value, (void*)value_pos, *length);
  400. switch (*type)
  401. {
  402. case TLV_TYPE_FLOAT64:
  403. net64f_to_host(*(double *)value);
  404. break;
  405. case TLV_TYPE_UINT64:
  406. case TLV_TYPE_INT64:
  407. net64_to_host(*(uint64_t *)value);
  408. break;
  409. case TLV_TYPE_FLOAT32:
  410. net32f_to_host(*(float *)value);
  411. break;
  412. case TLV_TYPE_UINT32:
  413. case TLV_TYPE_INT32:
  414. net32_to_host(*(uint32_t *)value);
  415. break;
  416. case TLV_TYPE_INT8:
  417. case TLV_TYPE_UINT8:
  418. case TLV_TYPE_BOOL:
  419. break;
  420. case TLV_TYPE_INT16:
  421. case TLV_TYPE_UINT16:
  422. net16_to_host(*(uint16_t *)value);
  423. break;
  424. default:
  425. break;
  426. }//switch
  427. }//need_length == 0
  428. return (struct TLV *)((uint8_t *)params_in + *length + sizeof(struct TLV)
  429. - (!need_length) * sizeof(*length));
  430. }
  431. /*******************************************************
  432. * Description:
  433. * param
  434. * return:
  435. *********************************************************/
  436. struct TLVs * FUNCTION_ATTRIBUTE get_sub_device_property(
  437. struct sub_device_buffer *device_buffer,
  438. struct pando_property *property_body)
  439. {
  440. struct pando_property *tmp_body = NULL;
  441. struct device_header *head = NULL;
  442. //reach the end of buffer
  443. if (s_current_property.position == device_buffer->buffer_length
  444. || s_current_property.position > device_buffer->buffer_length)
  445. {
  446. s_current_property.position = 0;
  447. return NULL;
  448. }
  449. if (s_current_property.position == 0)
  450. {
  451. //first property, need handle header length
  452. s_current_property.position += DEV_HEADER_LEN;
  453. head = (struct device_header *)device_buffer->buffer;
  454. base_params.data_sequence = net32_to_host(head->frame_seq);
  455. }
  456. tmp_body = (struct pando_property *)(device_buffer->buffer
  457. + s_current_property.position);
  458. s_current_property.position += sizeof(struct pando_property);
  459. return get_next_property(tmp_body, property_body);
  460. }
  461. uint16_t FUNCTION_ATTRIBUTE get_sub_device_payloadtype(struct sub_device_buffer *package)
  462. {
  463. struct device_header *head;
  464. if (package == NULL)
  465. {
  466. return -1;
  467. }
  468. head = (struct device_header *)package->buffer;
  469. return net16_to_host(head->payload_type);
  470. }
  471. uint8_t FUNCTION_ATTRIBUTE is_tlv_need_length(uint16_t type)
  472. {
  473. switch (type)
  474. {
  475. case TLV_TYPE_FLOAT64:
  476. case TLV_TYPE_FLOAT32:
  477. case TLV_TYPE_INT8:
  478. case TLV_TYPE_INT16:
  479. case TLV_TYPE_INT32:
  480. case TLV_TYPE_INT64:
  481. case TLV_TYPE_UINT8:
  482. case TLV_TYPE_UINT16:
  483. case TLV_TYPE_UINT32:
  484. case TLV_TYPE_UINT64:
  485. case TLV_TYPE_BOOL:
  486. return 0;
  487. break;
  488. case TLV_TYPE_BYTES:
  489. case TLV_TYPE_URI:
  490. return 1;
  491. break;
  492. default:
  493. pd_printf("Unknow data type.\n");
  494. return -1;
  495. break;
  496. }
  497. return -1;
  498. }
  499. uint8_t FUNCTION_ATTRIBUTE get_type_length(uint16_t type)
  500. {
  501. switch (type)
  502. {
  503. case TLV_TYPE_FLOAT64:
  504. case TLV_TYPE_UINT64:
  505. case TLV_TYPE_INT64:
  506. return 8;
  507. break;
  508. case TLV_TYPE_FLOAT32:
  509. case TLV_TYPE_UINT32:
  510. case TLV_TYPE_INT32:
  511. return 4;
  512. break;
  513. case TLV_TYPE_INT8:
  514. case TLV_TYPE_UINT8:
  515. case TLV_TYPE_BOOL:
  516. return 1;
  517. case TLV_TYPE_INT16:
  518. case TLV_TYPE_UINT16:
  519. return 2;
  520. break;
  521. case TLV_TYPE_BYTES:
  522. case TLV_TYPE_URI:
  523. return 0;
  524. break;
  525. default:
  526. pd_printf("Unknow data type.\n");
  527. return -1;
  528. break;
  529. }
  530. return -1;
  531. }
  532. /* malloc a new block to save old buffer and new property, and update the package length */
  533. int FUNCTION_ATTRIBUTE add_next_property(struct sub_device_buffer *data_package, uint16_t property_num, struct TLVs *next_data_params)
  534. {
  535. uint8_t *old_buffer = NULL;
  536. uint8_t *position = NULL;
  537. uint16_t old_len = 0;
  538. uint16_t subdevice_id;
  539. if (data_package == NULL)
  540. {
  541. return -1;
  542. }
  543. old_buffer = data_package->buffer;
  544. old_len = data_package->buffer_length;
  545. /* append payload length */
  546. data_package->buffer_length += (sizeof(struct pando_property)
  547. + current_tlv_block_size - sizeof(struct TLVs));
  548. data_package->buffer = (uint8_t *)pd_malloc(data_package->buffer_length);
  549. pd_memset(data_package->buffer, 0, data_package->buffer_length);
  550. /* copy old content and new property */
  551. position = copy_return_next(data_package->buffer, old_buffer, old_len);
  552. subdevice_id = 0;
  553. position = copy_return_next(position, &subdevice_id, sizeof(subdevice_id));
  554. property_num = host16_to_net(property_num);
  555. position = copy_return_next(position, &property_num, sizeof(property_num));
  556. pd_memcpy(position, next_data_params, current_tlv_block_size);
  557. pd_free(old_buffer);
  558. return 0;
  559. }
  560. int FUNCTION_ATTRIBUTE add_command(struct sub_device_buffer *command_package,
  561. uint16_t command_num, uint16_t priority, struct TLVs *command_params)
  562. {
  563. uint8_t *old_buffer = NULL;
  564. uint8_t *position = NULL;
  565. uint16_t old_len = 0;
  566. uint16_t subdevice_id;
  567. if (command_package == NULL)
  568. {
  569. return -1;
  570. }
  571. old_buffer = command_package->buffer;
  572. old_len = command_package->buffer_length;
  573. /* append payload length */
  574. command_package->buffer_length += (sizeof(struct pando_property)
  575. + current_tlv_block_size - sizeof(struct TLVs));
  576. command_package->buffer = (uint8_t *)pd_malloc(command_package->buffer_length);
  577. pd_memset(command_package->buffer, 0, command_package->buffer_length);
  578. /* copy old content and new property */
  579. position = copy_return_next(command_package->buffer, old_buffer, old_len);
  580. subdevice_id = 0;
  581. position = copy_return_next(position, &subdevice_id, sizeof(subdevice_id));
  582. command_num = host16_to_net(command_num);
  583. position = copy_return_next(position, &command_num, sizeof(command_num));
  584. priority = host16_to_net(priority);
  585. position = copy_return_next(position, &priority, sizeof(priority));
  586. pd_memcpy(position, command_params, current_tlv_block_size);
  587. pd_free(old_buffer);
  588. return 0;
  589. }
  590. int FUNCTION_ATTRIBUTE add_event(struct sub_device_buffer *event_package,
  591. uint16_t event_num, uint16_t priority, struct TLVs *event_params)
  592. {
  593. uint8_t *old_buffer = NULL;
  594. uint8_t *position = NULL;
  595. uint16_t old_len = 0;
  596. uint16_t subdevice_id;
  597. if (event_package == NULL)
  598. {
  599. return -1;
  600. }
  601. old_buffer = event_package->buffer;
  602. old_len = event_package->buffer_length;
  603. /* append payload length */
  604. event_package->buffer_length += (sizeof(struct pando_property)
  605. + current_tlv_block_size - sizeof(struct TLVs));
  606. event_package->buffer = (uint8_t *)pd_malloc(event_package->buffer_length);
  607. pd_memset(event_package->buffer, 0, event_package->buffer_length);
  608. /* copy old content and new property */
  609. position = copy_return_next(event_package->buffer, old_buffer, old_len);
  610. subdevice_id = 0;
  611. position = copy_return_next(position, &subdevice_id, sizeof(subdevice_id));
  612. event_num= host16_to_net(event_num);
  613. position = copy_return_next(position, &event_num, sizeof(event_num));
  614. priority = host16_to_net(priority);
  615. position = copy_return_next(position, &priority, sizeof(priority));
  616. pd_memcpy(position, event_params, current_tlv_block_size);
  617. pd_free(old_buffer);
  618. return 0;
  619. }
  620. int FUNCTION_ATTRIBUTE finish_package(struct sub_device_buffer *package_buf)
  621. {
  622. struct device_header *header;
  623. header = (struct device_header *)package_buf->buffer;
  624. header->payload_len = host16_to_net(package_buf->buffer_length
  625. - DEV_HEADER_LEN);
  626. header->crc = (header->payload_len) >> 8;
  627. if (header->payload_type == PAYLOAD_TYPE_DATA)
  628. {
  629. header->frame_seq = host32_to_net(base_params.data_sequence);
  630. }
  631. else if (header->payload_type == PAYLOAD_TYPE_COMMAND)
  632. {
  633. header->frame_seq = host32_to_net(base_params.command_sequence);
  634. }
  635. else if (header->payload_type == PAYLOAD_TYPE_EVENT)
  636. {
  637. header->frame_seq = host32_to_net(base_params.event_sequence);
  638. }
  639. return 0;
  640. }
  641. struct TLVs * FUNCTION_ATTRIBUTE get_next_property(
  642. struct pando_property *next_property, struct pando_property *property_body)
  643. {
  644. property_body->sub_device_id = net16_to_host(next_property->sub_device_id);
  645. property_body->property_num = net16_to_host(next_property->property_num);
  646. property_body->params->count = net16_to_host(next_property->params->count);
  647. return (struct TLVs *)((uint8_t *)next_property
  648. + sizeof(struct pando_property)
  649. - sizeof(struct TLVs));
  650. }
  651. int FUNCTION_ATTRIBUTE is_device_file_command(struct sub_device_buffer *device_buffer)
  652. {
  653. struct device_header *header = (struct device_header *)device_buffer->buffer;
  654. if (header->flags == 1)
  655. {
  656. return 1;
  657. }
  658. else
  659. {
  660. return 0;
  661. }
  662. }
  663. /*
  664. Temp region
  665. */
  666. uint8_t FUNCTION_ATTRIBUTE get_next_uint8(struct TLVs *params)
  667. {
  668. uint8_t val = 0;
  669. get_value(params, &val);
  670. return val;
  671. }
  672. uint16_t FUNCTION_ATTRIBUTE get_next_uint16(struct TLVs *params)
  673. {
  674. uint16_t val = 0;
  675. get_value(params, &val);
  676. return val;
  677. }
  678. uint32_t FUNCTION_ATTRIBUTE get_next_uint32(struct TLVs *params)
  679. {
  680. uint32_t val = 0;
  681. get_value(params, &val);
  682. return val;
  683. }
  684. uint64_t FUNCTION_ATTRIBUTE get_next_uint64(struct TLVs *params)
  685. {
  686. uint64_t val = 0;
  687. get_value(params, &val);
  688. return val;
  689. }
  690. int8_t FUNCTION_ATTRIBUTE get_next_int8(struct TLVs *params)
  691. {
  692. int8_t val = 0;
  693. get_value(params, &val);
  694. return val;
  695. }
  696. int16_t FUNCTION_ATTRIBUTE get_next_int16(struct TLVs *params)
  697. {
  698. int16_t val = 0;
  699. get_value(params, &val);
  700. return val;
  701. }
  702. int32_t FUNCTION_ATTRIBUTE get_next_int32(struct TLVs *params)
  703. {
  704. int32_t val = 0;
  705. get_value(params, &val);
  706. return val;
  707. }
  708. int64_t FUNCTION_ATTRIBUTE get_next_int64(struct TLVs *params)
  709. {
  710. int64_t val = 0;
  711. get_value(params, &val);
  712. return val;
  713. }
  714. float FUNCTION_ATTRIBUTE get_next_float32(struct TLVs *params)
  715. {
  716. float val = 0;
  717. get_value(params, &val);
  718. return val;
  719. }
  720. double FUNCTION_ATTRIBUTE get_next_float64(struct TLVs *params)
  721. {
  722. double val = 0;
  723. get_value(params, &val);
  724. return val;
  725. }
  726. uint8_t FUNCTION_ATTRIBUTE get_next_bool(struct TLVs *params)
  727. {
  728. uint8_t val = 0;
  729. get_value(params, &val);
  730. return val;
  731. }
  732. void FUNCTION_ATTRIBUTE *get_next_uri(struct TLVs *params, uint16_t *length)
  733. {
  734. return get_string(params,length);
  735. }
  736. void FUNCTION_ATTRIBUTE *get_next_bytes(struct TLVs *params, uint16_t *length)
  737. {
  738. return get_string(params,length);
  739. }
  740. int FUNCTION_ATTRIBUTE
  741. add_next_uint8(struct TLVs *params, uint8_t next_value)
  742. {
  743. return add_next_param(params, TLV_TYPE_UINT8, get_type_length(TLV_TYPE_INT8), &next_value);
  744. }
  745. int FUNCTION_ATTRIBUTE
  746. add_next_uint16(struct TLVs *params, uint16_t next_value)
  747. {
  748. return add_next_param(params, TLV_TYPE_UINT16, get_type_length(TLV_TYPE_UINT16), &next_value);
  749. }
  750. int FUNCTION_ATTRIBUTE
  751. add_next_uint32(struct TLVs *params, uint32_t next_value)
  752. {
  753. return add_next_param(params, TLV_TYPE_UINT32, get_type_length(TLV_TYPE_UINT32), &next_value);
  754. }
  755. int FUNCTION_ATTRIBUTE
  756. add_next_uint64(struct TLVs *params, uint64_t next_value)
  757. {
  758. return add_next_param(params, TLV_TYPE_UINT64, get_type_length(TLV_TYPE_UINT64), &next_value);
  759. }
  760. int FUNCTION_ATTRIBUTE
  761. add_next_int8(struct TLVs *params, int8_t next_value)
  762. {
  763. return add_next_param(params, TLV_TYPE_INT8, get_type_length(TLV_TYPE_INT8), &next_value);
  764. }
  765. int FUNCTION_ATTRIBUTE
  766. add_next_int16(struct TLVs *params, int16_t next_value)
  767. {
  768. return add_next_param(params, TLV_TYPE_INT16, get_type_length(TLV_TYPE_INT16), &next_value);
  769. }
  770. int FUNCTION_ATTRIBUTE
  771. add_next_int32(struct TLVs *params, int32_t next_value)
  772. {
  773. return add_next_param(params, TLV_TYPE_INT32, get_type_length(TLV_TYPE_INT32), &next_value);
  774. }
  775. int FUNCTION_ATTRIBUTE
  776. add_next_int64(struct TLVs *params, int64_t next_value)
  777. {
  778. return add_next_param(params, TLV_TYPE_INT64, get_type_length(TLV_TYPE_INT64), &next_value);
  779. }
  780. int FUNCTION_ATTRIBUTE
  781. add_next_float32(struct TLVs *params, float next_value)
  782. {
  783. return add_next_param(params, TLV_TYPE_FLOAT32, get_type_length(TLV_TYPE_FLOAT32), &next_value);
  784. }
  785. int FUNCTION_ATTRIBUTE
  786. add_next_float64(struct TLVs *params, double next_value)
  787. {
  788. return add_next_param(params, TLV_TYPE_FLOAT64, get_type_length(TLV_TYPE_FLOAT64), &next_value);
  789. }
  790. int FUNCTION_ATTRIBUTE
  791. add_next_bool(struct TLVs *params, uint8_t next_value)
  792. {
  793. return add_next_param(params, TLV_TYPE_BOOL, get_type_length(TLV_TYPE_BOOL), &next_value);
  794. }
  795. int FUNCTION_ATTRIBUTE
  796. add_next_uri(struct TLVs *params, uint16_t length, void *next_value)
  797. {
  798. return add_next_param(params, TLV_TYPE_URI, length, next_value);
  799. }
  800. int FUNCTION_ATTRIBUTE
  801. add_next_bytes(struct TLVs *params, uint16_t length, void *next_value)
  802. {
  803. return add_next_param(params, TLV_TYPE_BYTES, length, next_value);
  804. }