parser/msg_buf.c

   1 #include <linux/slab.h>
   2 #include <linux/vmalloc.h>
   3 #include "msg_buf.h"
   4 #include "parser_defs.h"
   5
   6 int init_mb(struct msg_buf *mb, size_t size)
   7 {
   8         if (size) {
   9                 mb->begin = vmalloc(size);
  10                 if (mb->begin == NULL) {
  11                         printk("Cannot alloc memory!\n");
  12                         return -ENOMEM;
  13                 }
  14
  15                 mb->ptr = mb->begin;
  16                 mb->end = mb->begin + size;
  17         } else
  18                 mb->begin = mb->end = mb->ptr = NULL;
  19
  20         return 0;
  21 }
  22
  23 void uninit_mb(struct msg_buf *mb)
  24 {
  25         vfree(mb->begin);
  26 }
  27
  28 int cmp_mb(struct msg_buf *mb, size_t size)
  29 {
  30         char *tmp;
  31
  32         tmp = mb->ptr + size;
  33         if (mb->end > tmp)
  34                 return 1;
  35         else if (mb->end < tmp)
  36                 return -1;
  37
  38         return 0;
  39 }
  40
  41 size_t remained_mb(struct msg_buf *mb)
  42 {
  43         return mb->end - mb->ptr;
  44 }
  45
  46 int is_end_mb(struct msg_buf *mb)
  47 {
  48         return mb->ptr == mb->end;
  49 }
  50
  51 int get_u32(struct msg_buf *mb, u32 *val)
  52 {
  53         if (cmp_mb(mb, sizeof(*val)) < 0)
  54                 return -EINVAL;
  55
  56         *val = *((u32 *)mb->ptr);
  57         mb->ptr += sizeof(*val);
  58
  59         print_parse_debug("u32->%d;%08X\n", *val, *val);
  60
  61         return 0;
  62 }
  63
  64 int get_u64(struct msg_buf *mb, u64 *val)
  65 {
  66         if (cmp_mb(mb, sizeof(*val)) < 0)
  67                 return -EINVAL;
  68
  69         *val = *((u64 *)mb->ptr);
  70         mb->ptr += sizeof(*val);
  71         print_parse_debug("u64->%d; 0x%016lX\n", *val, *val);
  72
  73         return 0;
  74 }
  75
  76 int get_string(struct msg_buf *mb, char **str)
  77 {
  78         size_t len, len_max;
  79
  80         len_max = mb->end - mb->ptr - 1;
  81         if(len_max < 0)
  82                 return -EINVAL;
  83
  84         len = strlen(mb->ptr) + 1;
  85
  86         *str = kmalloc(len, GFP_KERNEL);
  87         if (*str == NULL)
  88                 return -ENOMEM;
  89
  90         memcpy(*str, mb->ptr, len);
  91         mb->ptr += len;
  92
  93         print_parse_debug("str->'%s'\n", *str);
  94         return 0;
  95 }
  96
  97 void put_strung(char *str)
  98 {
  99         kfree(str);
 100 }