code.vuplus.com Git - vuplus_xbmc/blob - lib/libhdhomerun/hdhomerun_pkt.c

   1 /*
   2  * hdhomerun_pkt.c
   3  *
   4  * Copyright © 2005-2006 Silicondust Engineering Ltd. <www.silicondust.com>.
   5  *
   6  * This library is free software; you can redistribute it and/or
   7  * modify it under the terms of the GNU Lesser General Public
   8  * License as published by the Free Software Foundation; either
   9  * version 3 of the License, or (at your option) any later version.
  10  *
  11  * This library is distributed in the hope that it will be useful,
  12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  14  * Lesser General Public License for more details.
  15  *
  16  * You should have received a copy of the GNU Lesser General Public
  17  * License along with this library.  If not, see <http://www.gnu.org/licenses/>.
  18  *
  19  * As a special exception to the GNU Lesser General Public License,
  20  * you may link, statically or dynamically, an application with a
  21  * publicly distributed version of the Library to produce an
  22  * executable file containing portions of the Library, and
  23  * distribute that executable file under terms of your choice,
  24  * without any of the additional requirements listed in clause 4 of
  25  * the GNU Lesser General Public License.
  26  *
  27  * By "a publicly distributed version of the Library", we mean
  28  * either the unmodified Library as distributed by Silicondust, or a
  29  * modified version of the Library that is distributed under the
  30  * conditions defined in the GNU Lesser General Public License.
  31  */
  32
  33 #include "hdhomerun.h"
  34
  35 struct hdhomerun_pkt_t *hdhomerun_pkt_create(void)
  36 {
  37         struct hdhomerun_pkt_t *pkt = (struct hdhomerun_pkt_t *)calloc(1, sizeof(struct hdhomerun_pkt_t));
  38         if (!pkt) {
  39                 return NULL;
  40         }
  41
  42         hdhomerun_pkt_reset(pkt);
  43
  44         return pkt;
  45 }
  46
  47 void hdhomerun_pkt_destroy(struct hdhomerun_pkt_t *pkt)
  48 {
  49         free(pkt);
  50 }
  51
  52 void hdhomerun_pkt_reset(struct hdhomerun_pkt_t *pkt)
  53 {
  54         pkt->limit = pkt->buffer + sizeof(pkt->buffer) - 4;
  55         pkt->start = pkt->buffer + 1024;
  56         pkt->end = pkt->start;
  57         pkt->pos = pkt->start;
  58 }
  59
  60 static uint32_t hdhomerun_pkt_calc_crc(uint8_t *start, uint8_t *end)
  61 {
  62         uint8_t *pos = start;
  63         uint32_t crc = 0xFFFFFFFF;
  64         while (pos < end) {
  65                 uint8_t x = (uint8_t)(crc) ^ *pos++;
  66                 crc >>= 8;
  67                 if (x & 0x01) crc ^= 0x77073096;
  68                 if (x & 0x02) crc ^= 0xEE0E612C;
  69                 if (x & 0x04) crc ^= 0x076DC419;
  70                 if (x & 0x08) crc ^= 0x0EDB8832;
  71                 if (x & 0x10) crc ^= 0x1DB71064;
  72                 if (x & 0x20) crc ^= 0x3B6E20C8;
  73                 if (x & 0x40) crc ^= 0x76DC4190;
  74                 if (x & 0x80) crc ^= 0xEDB88320;
  75         }
  76         return crc ^ 0xFFFFFFFF;
  77 }
  78
  79 uint8_t hdhomerun_pkt_read_u8(struct hdhomerun_pkt_t *pkt)
  80 {
  81         uint8_t v = *pkt->pos++;
  82         return v;
  83 }
  84
  85 uint16_t hdhomerun_pkt_read_u16(struct hdhomerun_pkt_t *pkt)
  86 {
  87         uint16_t v;
  88         v =  (uint16_t)*pkt->pos++ << 8;
  89         v |= (uint16_t)*pkt->pos++ << 0;
  90         return v;
  91 }
  92
  93 uint32_t hdhomerun_pkt_read_u32(struct hdhomerun_pkt_t *pkt)
  94 {
  95         uint32_t v;
  96         v =  (uint32_t)*pkt->pos++ << 24;
  97         v |= (uint32_t)*pkt->pos++ << 16;
  98         v |= (uint32_t)*pkt->pos++ << 8;
  99         v |= (uint32_t)*pkt->pos++ << 0;
 100         return v;
 101 }
 102
 103 size_t hdhomerun_pkt_read_var_length(struct hdhomerun_pkt_t *pkt)
 104 {
 105         size_t length;
 106
 107         if (pkt->pos + 1 > pkt->end) {
 108                 return (size_t)-1;
 109         }
 110
 111         length = (size_t)*pkt->pos++;
 112         if (length & 0x0080) {
 113                 if (pkt->pos + 1 > pkt->end) {
 114                         return (size_t)-1;
 115                 }
 116
 117                 length &= 0x007F;
 118                 length |= (size_t)*pkt->pos++ << 7;
 119         }
 120
 121         return length;
 122 }
 123
 124 uint8_t *hdhomerun_pkt_read_tlv(struct hdhomerun_pkt_t *pkt, uint8_t *ptag, size_t *plength)
 125 {
 126         if (pkt->pos + 2 > pkt->end) {
 127                 return NULL;
 128         }
 129
 130         *ptag = hdhomerun_pkt_read_u8(pkt);
 131         *plength = hdhomerun_pkt_read_var_length(pkt);
 132
 133         if (pkt->pos + *plength > pkt->end) {
 134                 return NULL;
 135         }
 136
 137         return pkt->pos + *plength;
 138 }
 139
 140 void hdhomerun_pkt_write_u8(struct hdhomerun_pkt_t *pkt, uint8_t v)
 141 {
 142         *pkt->pos++ = v;
 143
 144         if (pkt->pos > pkt->end) {
 145                 pkt->end = pkt->pos;
 146         }
 147 }
 148
 149 void hdhomerun_pkt_write_u16(struct hdhomerun_pkt_t *pkt, uint16_t v)
 150 {
 151         *pkt->pos++ = (uint8_t)(v >> 8);
 152         *pkt->pos++ = (uint8_t)(v >> 0);
 153
 154         if (pkt->pos > pkt->end) {
 155                 pkt->end = pkt->pos;
 156         }
 157 }
 158
 159 void hdhomerun_pkt_write_u32(struct hdhomerun_pkt_t *pkt, uint32_t v)
 160 {
 161         *pkt->pos++ = (uint8_t)(v >> 24);
 162         *pkt->pos++ = (uint8_t)(v >> 16);
 163         *pkt->pos++ = (uint8_t)(v >> 8);
 164         *pkt->pos++ = (uint8_t)(v >> 0);
 165
 166         if (pkt->pos > pkt->end) {
 167                 pkt->end = pkt->pos;
 168         }
 169 }
 170
 171 void hdhomerun_pkt_write_var_length(struct hdhomerun_pkt_t *pkt, size_t v)
 172 {
 173         if (v <= 127) {
 174                 *pkt->pos++ = (uint8_t)v;
 175         } else {
 176                 *pkt->pos++ = (uint8_t)(v | 0x80);
 177                 *pkt->pos++ = (uint8_t)(v >> 7);
 178         }
 179
 180         if (pkt->pos > pkt->end) {
 181                 pkt->end = pkt->pos;
 182         }
 183 }
 184
 185 void hdhomerun_pkt_write_mem(struct hdhomerun_pkt_t *pkt, const void *mem, size_t length)
 186 {
 187         memcpy(pkt->pos, mem, length);
 188         pkt->pos += length;
 189
 190         if (pkt->pos > pkt->end) {
 191                 pkt->end = pkt->pos;
 192         }
 193 }
 194
 195 int hdhomerun_pkt_open_frame(struct hdhomerun_pkt_t *pkt, uint16_t *ptype)
 196 {
 197         pkt->pos = pkt->start;
 198
 199         if (pkt->pos + 4 > pkt->end) {
 200                 return 0;
 201         }
 202
 203         *ptype = hdhomerun_pkt_read_u16(pkt);
 204         size_t length = hdhomerun_pkt_read_u16(pkt);
 205         pkt->pos += length;
 206
 207         if (pkt->pos + 4 > pkt->end) {
 208                 pkt->pos = pkt->start;
 209                 return 0;
 210         }
 211
 212         uint32_t calc_crc = hdhomerun_pkt_calc_crc(pkt->start, pkt->pos);
 213
 214         uint32_t packet_crc;
 215         packet_crc =  (uint32_t)*pkt->pos++ << 0;
 216         packet_crc |= (uint32_t)*pkt->pos++ << 8;
 217         packet_crc |= (uint32_t)*pkt->pos++ << 16;
 218         packet_crc |= (uint32_t)*pkt->pos++ << 24;
 219         if (calc_crc != packet_crc) {
 220                 return -1;
 221         }
 222
 223         pkt->start += 4;
 224         pkt->end = pkt->start + length;
 225         pkt->pos = pkt->start;
 226         return 1;
 227 }
 228
 229 void hdhomerun_pkt_seal_frame(struct hdhomerun_pkt_t *pkt, uint16_t frame_type)
 230 {
 231         size_t length = pkt->end - pkt->start;
 232
 233         pkt->start -= 4;
 234         pkt->pos = pkt->start;
 235         hdhomerun_pkt_write_u16(pkt, frame_type);
 236         hdhomerun_pkt_write_u16(pkt, (uint16_t)length);
 237
 238         uint32_t crc = hdhomerun_pkt_calc_crc(pkt->start, pkt->end);
 239         *pkt->end++ = (uint8_t)(crc >> 0);
 240         *pkt->end++ = (uint8_t)(crc >> 8);
 241         *pkt->end++ = (uint8_t)(crc >> 16);
 242         *pkt->end++ = (uint8_t)(crc >> 24);
 243
 244         pkt->pos = pkt->start;
 245 }