drivers/staging/csr/csr_msgconv.c

   1 /*****************************************************************************
   2
   3             (c) Cambridge Silicon Radio Limited 2010
   4             All rights reserved and confidential information of CSR
   5
   6             Refer to LICENSE.txt included with this source for details
   7             on the license terms.
   8
   9 *****************************************************************************/
  10
  11 #include <linux/module.h>
  12 #include <linux/types.h>
  13 #include <linux/slab.h>
  14 #include "csr_panic.h"
  15 #include "csr_sched.h"
  16 #include "csr_msgconv.h"
  17 #include "csr_macro.h"
  18
  19 static CsrMsgConvEntry *converter;
  20
  21 CsrMsgConvPrimEntry *CsrMsgConvFind(u16 primType)
  22 {
  23     CsrMsgConvPrimEntry *ptr = NULL;
  24
  25     if (converter)
  26     {
  27         ptr = converter->profile_converters;
  28         while (ptr)
  29         {
  30             if (ptr->primType == primType)
  31             {
  32                 break;
  33             }
  34             else
  35             {
  36                 ptr = ptr->next;
  37             }
  38         }
  39     }
  40
  41     return ptr;
  42 }
  43
  44 static const CsrMsgConvMsgEntry *find_msg_converter(CsrMsgConvPrimEntry *ptr, u16 msgType)
  45 {
  46     const CsrMsgConvMsgEntry *cv = ptr->conv;
  47     if (ptr->lookupFunc)
  48     {
  49         return (const CsrMsgConvMsgEntry *) ptr->lookupFunc((CsrMsgConvMsgEntry *) cv, msgType);
  50     }
  51
  52     while (cv)
  53     {
  54         if (cv->serFunc == NULL)
  55         {
  56             /* We've reached the end of the chain */
  57             cv = NULL;
  58             break;
  59         }
  60
  61         if (cv->msgType == msgType)
  62         {
  63             break;
  64         }
  65         else
  66         {
  67             cv++;
  68         }
  69     }
  70
  71     return cv;
  72 }
  73
  74 static void *deserialize_data(u16 primType,
  75     size_t length,
  76     u8 *data)
  77 {
  78     CsrMsgConvPrimEntry *ptr;
  79     u8 *ret;
  80
  81     ptr = CsrMsgConvFind(primType);
  82
  83     if (ptr)
  84     {
  85         const CsrMsgConvMsgEntry *cv;
  86         u16 msgId = 0;
  87         size_t offset = 0;
  88         CsrUint16Des(&msgId, data, &offset);
  89
  90         cv = find_msg_converter(ptr, msgId);
  91         if (cv)
  92         {
  93             ret = cv->deserFunc(data, length);
  94         }
  95         else
  96         {
  97             ret = NULL;
  98         }
  99     }
 100     else
 101     {
 102         ret = NULL;
 103     }
 104
 105     return ret;
 106 }
 107
 108 static size_t sizeof_message(u16 primType, void *msg)
 109 {
 110     CsrMsgConvPrimEntry *ptr = CsrMsgConvFind(primType);
 111     size_t ret;
 112
 113     if (ptr)
 114     {
 115         const CsrMsgConvMsgEntry *cv;
 116         u16 msgId = *(u16 *) msg;
 117
 118         cv = find_msg_converter(ptr, msgId);
 119         if (cv)
 120         {
 121             ret = cv->sizeofFunc(msg);
 122         }
 123         else
 124         {
 125             ret = 0;
 126         }
 127     }
 128     else
 129     {
 130         ret = 0;
 131     }
 132
 133     return ret;
 134 }
 135
 136 static u8 free_message(u16 primType, u8 *data)
 137 {
 138     CsrMsgConvPrimEntry *ptr;
 139     u8 ret;
 140
 141     ptr = CsrMsgConvFind(primType);
 142
 143     if (ptr)
 144     {
 145         const CsrMsgConvMsgEntry *cv;
 146         u16 msgId = *(u16 *) data;
 147
 148         cv = find_msg_converter(ptr, msgId);
 149         if (cv)
 150         {
 151             cv->freeFunc(data);
 152             ret = TRUE;
 153         }
 154         else
 155         {
 156             ret = FALSE;
 157         }
 158     }
 159     else
 160     {
 161         ret = FALSE;
 162     }
 163
 164     return ret;
 165 }
 166
 167 static u8 *serialize_message(u16 primType,
 168     void *msg,
 169     size_t *length,
 170     u8 *buffer)
 171 {
 172     CsrMsgConvPrimEntry *ptr;
 173     u8 *ret;
 174
 175     ptr = CsrMsgConvFind(primType);
 176
 177     *length = 0;
 178
 179     if (ptr)
 180     {
 181         const CsrMsgConvMsgEntry *cv;
 182
 183         cv = find_msg_converter(ptr, *(u16 *) msg);
 184         if (cv)
 185         {
 186             ret = cv->serFunc(buffer, length, msg);
 187         }
 188         else
 189         {
 190             ret = NULL;
 191         }
 192     }
 193     else
 194     {
 195         ret = NULL;
 196     }
 197
 198     return ret;
 199 }
 200
 201 size_t CsrMsgConvSizeof(u16 primType, void *msg)
 202 {
 203     return sizeof_message(primType, msg);
 204 }
 205
 206 u8 *CsrMsgConvSerialize(u8 *buffer, size_t maxBufferOffset, size_t *offset, u16 primType, void *msg)
 207 {
 208     if (converter)
 209     {
 210         size_t serializedLength;
 211         u8 *bufSerialized;
 212         u8 *bufOffset = &buffer[*offset];
 213         bufSerialized = converter->serialize_message(primType, msg, &serializedLength, bufOffset);
 214         *offset += serializedLength;
 215         return bufSerialized;
 216     }
 217     else
 218     {
 219         return NULL;
 220     }
 221 }
 222
 223 /* Insert profile converter at head of converter list. */
 224 void CsrMsgConvInsert(u16 primType, const CsrMsgConvMsgEntry *ce)
 225 {
 226     CsrMsgConvPrimEntry *pc;
 227     pc = CsrMsgConvFind(primType);
 228
 229     if (pc)
 230     {
 231         /* Already registered. Do nothing */
 232     }
 233     else
 234     {
 235         pc = kmalloc(sizeof(*pc), GFP_KERNEL);
 236         pc->primType = primType;
 237         pc->conv = ce;
 238         pc->lookupFunc = NULL;
 239         pc->next = converter->profile_converters;
 240         converter->profile_converters = pc;
 241     }
 242 }
 243 EXPORT_SYMBOL_GPL(CsrMsgConvInsert);
 244
 245 CsrMsgConvMsgEntry *CsrMsgConvFindEntry(u16 primType, u16 msgType)
 246 {
 247     CsrMsgConvPrimEntry *ptr = CsrMsgConvFind(primType);
 248     if (ptr)
 249     {
 250         return (CsrMsgConvMsgEntry *) find_msg_converter(ptr, msgType);
 251     }
 252     return NULL;
 253 }
 254 EXPORT_SYMBOL_GPL(CsrMsgConvFindEntry);
 255
 256 CsrMsgConvMsgEntry *CsrMsgConvFindEntryByMsg(u16 primType, const void *msg)
 257 {
 258     CsrMsgConvPrimEntry *ptr = CsrMsgConvFind(primType);
 259     if (ptr && msg)
 260     {
 261         u16 msgType = *((u16 *) msg);
 262         return (CsrMsgConvMsgEntry *) find_msg_converter(ptr, msgType);
 263     }
 264     return NULL;
 265 }
 266
 267 void CsrMsgConvCustomLookupRegister(u16 primType, CsrMsgCustomLookupFunc *lookupFunc)
 268 {
 269     CsrMsgConvPrimEntry *ptr = CsrMsgConvFind(primType);
 270     if (ptr)
 271     {
 272         ptr->lookupFunc = lookupFunc;
 273     }
 274 }
 275 EXPORT_SYMBOL_GPL(CsrMsgConvCustomLookupRegister);
 276
 277 CsrMsgConvEntry *CsrMsgConvInit(void)
 278 {
 279     if (!converter)
 280     {
 281         converter = kmalloc(sizeof(CsrMsgConvEntry), GFP_KERNEL);
 282
 283         converter->profile_converters = NULL;
 284         converter->free_message = free_message;
 285         converter->sizeof_message = sizeof_message;
 286         converter->serialize_message = serialize_message;
 287         converter->deserialize_data = deserialize_data;
 288     }
 289
 290     return converter;
 291 }
 292 EXPORT_SYMBOL_GPL(CsrMsgConvInit);