src/util_mfld_blackbay.c

   1 /*\r
   2 *\r
   3 *  tel-plugin-mfld-blackbay\r
   4 *\r
   5 * Copyright (C) 2013  Intel Corporation. All rights reserved.\r
   6 *\r
   7 * Licensed under the Apache License, Version 2.0 (the "License");\r
   8 * you may not use this file except in compliance with the License.\r
   9 * You may obtain a copy of the License at\r
  10 *\r
  11 *     http://www.apache.org/licenses/LICENSE-2.0\r
  12 *\r
  13 * Unless required by applicable law or agreed to in writing, software\r
  14 * distributed under the License is distributed on an "AS IS" BASIS,\r
  15 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.\r
  16 * See the License for the specific language governing permissions and\r
  17 * limitations under the License.\r
  18 *\r
  19 */\r
  20 \r
  21 #include <string.h>\r
  22 #include <stdlib.h>\r
  23 #include <stdio.h>\r
  24 #include <errno.h>\r
  25 #include <sys/time.h>\r
  26 #include <sys/mman.h>\r
  27 #include <unistd.h>\r
  28 #include <fcntl.h>\r
  29 \r
  30 #include <glib.h>\r
  31 \r
  32 #include <log.h>\r
  33 #include <util_mfld_blackbay.h>\r
  34 \r
  35 #define MAX_READ_ATTEMPTS       4\r
  36 #define MAX_WRITE_ATTEMPTS      10\r
  37 \r
  38 /* Read data from a GIOChannel */\r
  39 size_t channel_read(GIOChannel *channel, void* buf, size_t nbytes)\r
  40 {\r
  41         GIOStatus status;\r
  42         size_t rbytes;\r
  43         size_t toread = nbytes;\r
  44         size_t bytes_read = 0;\r
  45         unsigned int read_count = 0;\r
  46 \r
  47         do {\r
  48                 rbytes = 0;\r
  49 \r
  50                 status = g_io_channel_read_chars(channel, buf, toread, &rbytes, NULL);\r
  51 \r
  52                 read_count++;\r
  53 \r
  54                 dbg("g_io_channel_read_chars.. status[%d] rbytes =%d read_count = %d", status, rbytes, read_count);\r
  55 \r
  56                 bytes_read += rbytes;\r
  57                 toread -= rbytes;\r
  58 \r
  59                 if (rbytes > 0)\r
  60                         buf+=rbytes;\r
  61 \r
  62                 if (toread == 0)\r
  63                         break;\r
  64 \r
  65         } while (status == G_IO_STATUS_NORMAL && rbytes > 0 && read_count < MAX_READ_ATTEMPTS);\r
  66 \r
  67         return bytes_read;\r
  68 }\r
  69 \r
  70 static void __selectsleep(int sec,int msec)\r
  71 {\r
  72         struct timeval tv;\r
  73 \r
  74         tv.tv_sec=sec;\r
  75         tv.tv_usec=msec;\r
  76 \r
  77         select(0,NULL,NULL,NULL,&tv);\r
  78 \r
  79         return;\r
  80 }\r
  81 \r
  82 /* Write data to a GIOChannel */\r
  83 size_t channel_write(GIOChannel *channel, void* buf, size_t nbytes)\r
  84 {\r
  85         GIOStatus status;\r
  86         size_t tbytes = 0;\r
  87         size_t bytes_written = 0;\r
  88         unsigned int retry = 0;\r
  89 \r
  90         do {\r
  91                 status = g_io_channel_write_chars(channel, (const gchar*)buf, nbytes - bytes_written, &tbytes, NULL);\r
  92 \r
  93                 if (status == G_IO_STATUS_AGAIN) {\r
  94                         err("write failed. Retry.. status[%d] Nb attempt[%d]", status, retry);\r
  95 \r
  96                         __selectsleep(0,50);\r
  97 \r
  98                         if (retry == MAX_WRITE_ATTEMPTS)\r
  99                                 return 0;\r
 100 \r
 101                         retry = retry + 1;\r
 102 \r
 103                         continue;\r
 104                 }\r
 105 \r
 106                 if (status != G_IO_STATUS_NORMAL) {\r
 107                         if (bytes_written != nbytes)\r
 108                                 err("write failed. status[%d] with errno[%d]", status, errno);\r
 109 \r
 110                         return bytes_written;\r
 111                 }\r
 112 \r
 113                 bytes_written += tbytes;\r
 114                 buf += tbytes;\r
 115 \r
 116         } while (bytes_written < nbytes);\r
 117 \r
 118         return bytes_written;\r
 119 }\r
 120 \r
 121 void util_hex_dump(char *pad, int size, const void *data)\r
 122 {\r
 123         char buf[255] = {0, };\r
 124         char hex[4] = {0, };\r
 125         int i;\r
 126         unsigned char *p;\r
 127 \r
 128         if (size <= 0) {\r
 129                 dbg("<%s> no data", pad);\r
 130                 return;\r
 131         }\r
 132 \r
 133         p = (unsigned char *) data;\r
 134 \r
 135         snprintf(buf, 255, "%s%04X: ", pad, 0);\r
 136         for (i = 0; i < size; i++) {\r
 137                 snprintf(hex, 4, "%02X ", p[i]);\r
 138                 strcat(buf, hex);\r
 139 \r
 140                 if ((i + 1) % 8 == 0) {\r
 141                         if ((i + 1) % 16 == 0) {\r
 142                                 msg("%s", buf);\r
 143                                 memset(buf, 0, 255);\r
 144                                 snprintf(buf, 255, "%s%04X: ", pad, i + 1);\r
 145                         } else {\r
 146                                 strcat(buf, "  ");\r
 147                         }\r
 148                 }\r
 149         }\r
 150 \r
 151         dbg("<%s>", buf);\r
 152 }\r
 153 \r