src/imc_common.c

   1 /*
   2  * tel-plugin-imc
   3  *
   4  * Copyright (c) 2012 Samsung Electronics Co., Ltd. All rights reserved.
   5  *
   6  * Contact: Ja-young Gu <jygu@samsung.com>
   7  *
   8  * Licensed under the Apache License, Version 2.0 (the "License");
   9  * you may not use this file except in compliance with the License.
  10  * You may obtain a copy of the License at
  11  *
  12  * http://www.apache.org/licenses/LICENSE-2.0
  13  *
  14  * Unless required by applicable law or agreed to in writing, software
  15  * distributed under the License is distributed on an "AS IS" BASIS,
  16  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  17  * See the License for the specific language governing permissions and
  18  * limitations under the License.
  19  */
  20
  21 #include <stdio.h>
  22 #include <string.h>
  23 #include <stdlib.h>
  24
  25 #include <glib.h>
  26 #include <log.h>
  27
  28
  29 #include "imc_common.h"
  30
  31 #undef  MAX
  32 #define MAX(a, b)  (((a) > (b)) ? (a) : (b))
  33
  34 #undef  MIN
  35 #define MIN(a, b)  (((a) < (b)) ? (a) : (b))
  36
  37 #define bitsize(type) (sizeof(type) * 8)
  38
  39 #define copymask(type) ((0xffffffff) >> (32 - bitsize(type)))
  40
  41 #define MASK(width, offset, data) \
  42         (((width) == bitsize(data)) ? (data) :   \
  43          ((((copymask(data) << (bitsize(data) - ((width) % bitsize(data)))) & copymask(data)) >> (offset)) & (data))) \
  44
  45
  46 #define MASK_AND_SHIFT(width, offset, shift, data)      \
  47         ((((signed) (shift)) < 0) ?               \
  48          MASK((width), (offset), (data)) << -(shift) :  \
  49          MASK((width), (offset), (data)) >> (((signed) (shift)))) \
  50
  51 char _util_unpackb(const char *src, int pos, int len);
  52 char _util_convert_byte_hexChar(char val);
  53 gboolean util_byte_to_hex(const char *byte_pdu, char *hex_pdu, int num_bytes);
  54
  55 unsigned char util_hexCharToInt(char c)
  56 {
  57         if (c >= '0' && c <= '9')
  58                 return (c - '0');
  59         else if (c >= 'A' && c <= 'F')
  60                 return (c - 'A' + 10);
  61         else if (c >= 'a' && c <= 'f')
  62                 return (c - 'a' + 10);
  63         else {
  64                 dbg("invalid charater!!");
  65                 return -1;
  66         }
  67 }
  68
  69 char *util_hex_to_string(const char *src, unsigned int src_len)
  70 {
  71         char *dest;
  72         unsigned int i;
  73
  74         if (src == NULL)
  75                 return NULL;
  76
  77         dest = g_malloc0(src_len * 2 + 1);
  78         if (dest == NULL) {
  79                 err("Memory allocation failed!!");
  80                 return NULL;
  81         }
  82
  83         for (i = 0; i < src_len; i++) {
  84                 snprintf(dest + (i * 2), (src_len * 2 + 1) - (i * 2),
  85                         "%02x", (unsigned char)src[i]);
  86         }
  87
  88         dest[src_len * 2] = '\0';
  89
  90         return dest;
  91 }
  92
  93 char *util_hexStringToBytes(char *s)
  94 {
  95         char *ret;
  96         int i;
  97         int sz;
  98
  99         if (s == NULL)
 100                 return NULL;
 101
 102         sz = strlen(s);
 103
 104         ret = g_malloc0((sz / 2) + 1);
 105         if (ret == NULL) {
 106                 err("Memory allocation failed!!");
 107                 return NULL;
 108         }
 109
 110         dbg("Convert String to Binary!!");
 111
 112         for (i = 0; i < sz; i += 2) {
 113                 ret[i / 2] = (char) ((util_hexCharToInt(s[i]) << 4) | util_hexCharToInt(s[i + 1]));
 114                 msg("           [%02x]", ret[i / 2]);
 115         }
 116
 117         return ret;
 118 }
 119
 120 char _util_unpackb(const char *src, int pos, int len)
 121 {
 122         char result = 0;
 123         int rshift = 0;
 124
 125         src += pos / 8;
 126         pos %= 8;
 127
 128         rshift = MAX(8 - (pos + len), 0);
 129
 130         if (rshift > 0) {
 131                 result = MASK_AND_SHIFT(len, pos, rshift, (unsigned char)*src);
 132         } else {
 133                 result = MASK(8 - pos, pos, (unsigned char)*src);
 134                 src++;
 135                 len -= 8 - pos;
 136
 137                 if (len > 0) result = (result << len) | (*src >> (8 - len));   /* if any bits left */
 138         }
 139
 140         return result;
 141 }
 142
 143 char _util_convert_byte_hexChar(char val)
 144 {
 145         char hex_char;
 146
 147         if (val <= 9)
 148                 hex_char = (char) (val + '0');
 149         else if (val >= 10 && val <= 15)
 150                 hex_char = (char) (val - 10 + 'A');
 151         else
 152                 hex_char = '0';
 153
 154         return (hex_char);
 155 }
 156
 157 gboolean util_byte_to_hex(const char *byte_pdu, char *hex_pdu, int num_bytes)
 158 {
 159         int i;
 160         char nibble;
 161         int buf_pos = 0;
 162
 163         for (i = 0; i < num_bytes * 2; i++) {
 164                 nibble = _util_unpackb(byte_pdu, buf_pos, 4);
 165                 buf_pos += 4;
 166                 hex_pdu[i] = _util_convert_byte_hexChar(nibble);
 167         }
 168
 169         return TRUE;
 170 }