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