/* Cesar project {{{ * * Copyright (C) 2008 Spidcom * * <<>> * * }}} */ /** * \file cp/msg/src/msg_drv.c * \brief « brief description » * \ingroup « module » * * « long description » */ #include #include #include "common/std.h" #include "lib/bitstream.h" #include "cp/secu/inc/secu_types.h" #include "cp/msg/inc/msg_const.h" #include "cp/msg/inc/msg_drv.h" const char *cst_str_drv_mme_cnf_result[] = { "FAILURE", "SUCCESS" }; const char *cst_str_drv_mme_cnf_errcode[] = { "BAD PARAMETER", "UNKNOWN ID", "INVALID VALUE" }; const char *cst_str_yes_no[] = { "no", "yes" }; /** * \brief Get DRV-MME .CNF message result string * * \param cnf_result the result code * \return the result code string constant */ char * get_str_drv_mme_cnf_result(int cnf_result) { return (char*)cst_str_drv_mme_cnf_result[cnf_result]; } /** * \brief Get DRV-MME .CNF message error code string * * \param cnf_errcode the error code * \return the error code string constant */ char * get_str_drv_mme_cnf_errcode(int cnf_errcode) { return (char*)cst_str_drv_mme_cnf_errcode[cnf_errcode]; } /** * \brief Get yes/no string. * * \param no_yes the index of yes/no string array * \return the yes or no string constant */ char * get_str_yes_no(int no_yes) { return (char*)cst_str_yes_no[no_yes]; } /** * \brief Verify a password or HFID string validity. * * \param str the HFID string to be verified * \param minlen the minimal length * \param maxlen the maximal length * \return true or false depending on validity ok or not */ bool check_password_or_hfid_is_valid(char *str, int minlen,int maxlen) { int i; if (((int)strlen(str) < minlen) || ((int)strlen(str) > maxlen)) return false; for (i = 0; i < (int)strlen(str); i++) { if (((u8)str[i] < MIN_PWD_ASCII_CHAR) || ((u8)str[i] > MAX_PWD_ASCII_CHAR)) { return false; } } return true; } /** * \brief little endian to CPU host endian conversion. * * \param addrval pointer to the value to be (re)formatted from little endian to CPU host endian * \param bytesizeval the size in bytes of the value */ void little_to_cpuhost_endian(u8 *addrval, int bytesizeval) { u8 c; int i; union { u32 u32; u8 b[4]; } cpuarch; cpuarch.u32 = 1; if (cpuarch.b[0] != 1) { //printf("BIG-ENDIAN CPU...\n"); for (i = 0; i < (bytesizeval / 2); i++) { c = addrval[i]; addrval[i] = addrval[bytesizeval-1-i]; //printf("[%d] <- 0x%02X ; [%d] <- 0x%02X\n",i,addrval[bytesizeval-1-i],bytesizeval-1-i,c); addrval[bytesizeval-1-i] = c; } } } /** * \brief big endian to CPU host endian conversion. * * \param addrval pointer to the value to be (re)formatted from big endian to CPU host endian * \param bytesizeval the size in bytes of the value */ void big_to_cpuhost_endian(u8 *addrval, int bytesizeval) { u8 c; int i; union { u32 u32; u8 b[4]; } cpuarch; cpuarch.u32 = 1; if (cpuarch.b[0] == 1) { //printf("LITTLE-ENDIAN CPU...\n"); for (i = 0; i < (bytesizeval / 2); i++) { c = addrval[i]; addrval[i] = addrval[bytesizeval-1-i]; //printf("[%d] <- 0x%02X ; [%d] <- 0x%02X\n",i,addrval[bytesizeval-1-i],bytesizeval-1-i,c); addrval[bytesizeval-1-i] = c; } } } /** * \brief CPU host endian to little endian conversion. * * \param addrval pointer to the value to be (re)formatted from CPU host endian to little endian * \param bytesizeval the size in bytes of the value */ void cpuhost_to_little_endian(u8 *addrval, int bytesizeval) { u8 c; int i; union { u32 u32; u8 b[4]; } cpuarch; cpuarch.u32 = 1; if (cpuarch.b[0] != 1) { //printf("BIG-ENDIAN CPU...\n"); for (i = 0; i < (bytesizeval / 2); i++) { c = addrval[i]; addrval[i] = addrval[bytesizeval-1-i]; //printf("[%d] <- 0x%02X ; [%d] <- 0x%02X\n",i,addrval[bytesizeval-1-i],bytesizeval-1-i,c); addrval[bytesizeval-1-i] = c; } } } /** * \brief CPU host endian to big endian conversion. * * \param addrval pointer to the value to be (re)formatted from CPU host endian to big endian * \param bytesizeval the size in bytes of the value */ void cpuhost_to_big_endian(u8 *addrval, int bytesizeval) { u8 c; int i; union { u32 u32; u8 b[4]; } cpuarch; cpuarch.u32 = 1; if (cpuarch.b[0] == 1) { //printf("LITTLE-ENDIAN CPU...\n"); for (i = 0; i < (bytesizeval / 2); i++) { c = addrval[i]; addrval[i] = addrval[bytesizeval-1-i]; //printf("[%d] <- 0x%02X ; [%d] <- 0x%02X\n",i,addrval[bytesizeval-1-i],bytesizeval-1-i,c); addrval[bytesizeval-1-i] = c; } } } u16 big_to_cpuhost_endian_u16(u16 val) { u16 ret = val; big_to_cpuhost_endian((u8 *)&ret, sizeof(ret)); return ret; } u32 big_to_cpuhost_endian_u32(u32 val) { u32 ret = val; big_to_cpuhost_endian((u8 *)&ret, sizeof(ret)); return ret; } u16 cpuhost_to_big_endian_u16(u16 val) { u16 ret = val; cpuhost_to_big_endian((u8 *)&ret, sizeof(ret)); return ret; } u32 cpuhost_to_big_endian_u32(u32 val) { u32 ret = val; cpuhost_to_big_endian((u8 *)&ret, sizeof(ret)); return ret; } void display_mme_header(mme_header_type *mme_header) { printf ("%u /*\t0x%08x : osa ........ 0x%012llX => @MAC %02x:%02x:%02x:%02x:%02x:%02x\n", cyg_hal_sys_getpid(), (unsigned int)&mme_header->un_osa, mme_header->un_osa.u64, (u8)((mme_header->un_osa.u64 & 0xFF0000000000ULL) >> 40), (u8)((mme_header->un_osa.u64 & 0x00FF00000000ULL) >> 32), (u8)((mme_header->un_osa.u64 & 0x0000FF000000ULL) >> 24), (u8)((mme_header->un_osa.u64 & 0x000000FF0000ULL) >> 16), (u8)((mme_header->un_osa.u64 & 0x00000000FF00ULL) >> 8), (u8) (mme_header->un_osa.u64 & 0x0000000000FFULL) ); printf ("%u /*\t0x%08x : oda ........ 0x%012llX => @MAC %02x:%02x:%02x:%02x:%02x:%02x\n", cyg_hal_sys_getpid(), (unsigned int)&mme_header->un_oda, mme_header->un_oda.u64, (u8)((mme_header->un_oda.u64 & 0xFF0000000000ULL) >> 40), (u8)((mme_header->un_oda.u64 & 0x00FF00000000ULL) >> 32), (u8)((mme_header->un_oda.u64 & 0x0000FF000000ULL) >> 24), (u8)((mme_header->un_oda.u64 & 0x000000FF0000ULL) >> 16), (u8)((mme_header->un_oda.u64 & 0x00000000FF00ULL) >> 8), (u8) (mme_header->un_oda.u64 & 0x0000000000FFULL) ); if (mme_header->vlan_tag_flag == true) { printf ("%u /*\t0x%08x : vlan_tag ... 0x%04X 0x%04X", cyg_hal_sys_getpid(), (unsigned int)&mme_header->un_vlan_tag, (u16)(mme_header->un_vlan_tag.u32 >> 16), (u16)(mme_header->un_vlan_tag.u32 & 0x0000FFFF)); if ((u16)(mme_header->un_vlan_tag.u32 >> 16) == MSG_VLAN_TAG_IEEE_802_1Q) printf(" => IEEE 802.1Q VLAN Tag\n"); else printf(" => UNKNOWN/UNEXPECTED !?\n"); } printf ("%u /*\t0x%08x : mtype ...... 0x%04lX", cyg_hal_sys_getpid(), (unsigned int)&mme_header->un_mtype, mme_header->un_mtype.u32); if (mme_header->un_mtype.u32 == MSG_MTYPE_IEEE_ETHERTYPE) printf(" => IEEE ETHERTYPE\n"); else printf(" => UNKNOWN/UNEXPECTED !?\n"); printf ("%u /*\t0x%08x : mmv ........ 0x%02lX (%lu) => HomePlug AV 1.%lu specification\n", cyg_hal_sys_getpid(), (unsigned int)&mme_header->un_mmv, mme_header->un_mmv.u32,mme_header->un_mmv.u32, mme_header->un_mmv.u32); printf ("%u /*\t0x%08x : mmtype ..... 0x%04lX", cyg_hal_sys_getpid(), (unsigned int)&mme_header->un_mmtype, mme_header->un_mmtype.u32); switch(mme_header->un_mmtype.u32) { case DRV_STA_SET_MAC_ADDR_REQ : case DRV_STA_SET_CCO_PREF_REQ : case DRV_STA_SET_WAS_CCO_REQ : case DRV_STA_SET_NPW_REQ : case DRV_STA_SET_DPW_REQ : case DRV_STA_SET_SL_REQ : case DRV_STA_SET_M_STA_HFID_REQ : case DRV_STA_SET_U_STA_HFID_REQ : case DRV_STA_SET_AVLN_HFID_REQ : case DRV_STA_SET_TONEMASK_REQ : case DRV_STA_START_REQ : case DRV_STA_STOP_REQ : printf(" => %s\n",get_str_drv_mme_req((mme_header->un_mmtype.u32-DRV_STA_SET_MAC_ADDR_REQ)/4)); break; case DRV_STA_SET_MAC_ADDR_CNF : case DRV_STA_SET_CCO_PREF_CNF : case DRV_STA_SET_WAS_CCO_CNF : case DRV_STA_SET_NPW_CNF : case DRV_STA_SET_DPW_CNF : case DRV_STA_SET_SL_CNF : case DRV_STA_SET_M_STA_HFID_CNF : case DRV_STA_SET_U_STA_HFID_CNF : case DRV_STA_SET_AVLN_HFID_CNF : case DRV_STA_SET_TONEMASK_CNF : case DRV_STA_START_CNF : case DRV_STA_STOP_CNF : printf(" => %s\n",get_str_drv_mme_cnf((mme_header->un_mmtype.u32-DRV_STA_SET_MAC_ADDR_CNF)/4)); break; default : printf("\n"); break; } printf ("%u /*\t0x%08x : fmi ........ 0x%04lX => MMENTRY fragment %lu/%lu, FNSM = 0x%02lX (%lu)\n", cyg_hal_sys_getpid(), (unsigned int)&mme_header->un_fmi, mme_header->un_fmi.u32, ((mme_header->un_fmi.u32 & 0xF000) >> 12)+1, ((mme_header->un_fmi.u32 & 0xF00) >> 8)+1, mme_header->un_fmi.u32 & 0xFF, mme_header->un_fmi.u32 & 0xFF); } /** * \brief Build a "MME.CNF" confirmation message. * * \param recv_mme_header the buffer to contain the MME.CNF message being built * \param send_mme the buffer containing the MME initially received for which we're building a CNF * \param cnf_result the result code of the MME.CNF message to be built * \param cnf_errcode the error code of the MME.CNF message to be built * \param cnf_len the returned length of the MME.CNF message being built * */ void cp_mme_build_cnf (mme_header_type *recv_mme_header, mme_drv_cnf_type *send_mme, e_drv_mme_cnf_result cnf_result, e_drv_mme_cnf_errcode cnf_errcode, uint *cnf_len, u8 *mme_tx_buffer, int *mme_buffer_first) { bitstream_t bstr_ctx; uint pld_maxlen; uint i, j; printf ("%u %s() : mme_tx_buffer = 0x%08lx\n",cyg_hal_sys_getpid(),__FUNCTION__,(unsigned long)mme_tx_buffer); /* Prepare CNF message to be sent back to OSA */ send_mme->header = *recv_mme_header; send_mme->header.un_osa.u64 = recv_mme_header->un_oda.u64; send_mme->header.un_oda.u64 = recv_mme_header->un_osa.u64; send_mme->header.un_mmtype.u32 |= CNF; /* TODO : don't reset the VLAN tag field, keep the one of the incoming message header... */ send_mme->header.un_vlan_tag.u32 = 0x00000000; /* TODO : fill & format the FMI field... */ send_mme->header.un_fmi.u32 = 0x00000000; printf ("%u /* MME HEADER :\n",cyg_hal_sys_getpid()); display_mme_header(&send_mme->header); printf ("%u /***************************************************/\n",cyg_hal_sys_getpid()); switch(send_mme->header.un_mmtype.u32) { case DRV_STA_SET_MAC_ADDR_CNF : case DRV_STA_SET_CCO_PREF_CNF : case DRV_STA_SET_WAS_CCO_CNF : case DRV_STA_SET_NPW_CNF : case DRV_STA_SET_DPW_CNF : case DRV_STA_SET_SL_CNF : case DRV_STA_SET_M_STA_HFID_CNF : case DRV_STA_SET_U_STA_HFID_CNF : case DRV_STA_SET_AVLN_HFID_CNF : case DRV_STA_SET_TONEMASK_CNF : case DRV_STA_START_CNF : case DRV_STA_STOP_CNF : pld_maxlen = 2; break; default : pld_maxlen = 0; /* TO DO : what length ? */ break; } printf ("%u /* MME PAYLOAD :\n",cyg_hal_sys_getpid()); switch(send_mme->header.un_mmtype.u32) { case DRV_STA_SET_MAC_ADDR_CNF : case DRV_STA_SET_CCO_PREF_CNF : case DRV_STA_SET_WAS_CCO_CNF : case DRV_STA_SET_NPW_CNF : case DRV_STA_SET_DPW_CNF : case DRV_STA_SET_SL_CNF : case DRV_STA_SET_M_STA_HFID_CNF : case DRV_STA_SET_U_STA_HFID_CNF : case DRV_STA_SET_AVLN_HFID_CNF : case DRV_STA_SET_TONEMASK_CNF : case DRV_STA_START_CNF : case DRV_STA_STOP_CNF : send_mme->result = cnf_result; send_mme->errcode = cnf_errcode; printf ("%u /*\t0x%08x : result ..... 0x%02X (%u) => %s\n",cyg_hal_sys_getpid(),(unsigned int)&send_mme->result, send_mme->result,send_mme->result,get_str_drv_mme_cnf_result(send_mme->result)); printf ("%u /*\t0x%08x : errcode .... 0x%02X (%u)",cyg_hal_sys_getpid(),(unsigned int)&send_mme->errcode, send_mme->errcode,send_mme->errcode); if (send_mme->result != E_DRVMME_RESULT_SUCCESS) printf (" => %s\n",get_str_drv_mme_cnf_errcode(send_mme->errcode)); else printf ("\n"); break; default : printf("\n"); break; } printf ("%u /***************************************************/\n",cyg_hal_sys_getpid()); /* re-convert needed fields to big-endian... */ cpuhost_to_big_endian(&send_mme->header.un_osa.b[0],MAC_ADDR_SIZE); cpuhost_to_big_endian(&send_mme->header.un_oda.b[0],MAC_ADDR_SIZE); cpuhost_to_big_endian(&send_mme->header.un_vlan_tag.b[0],4); cpuhost_to_big_endian(&send_mme->header.un_mtype.b[0],2); /* * take into account right mme header len depending on VLAN Tag field * was present in incoming message or not (reply message header conforms * to incoming message header) */ if (!send_mme->header.vlan_tag_flag) { *cnf_len = (uint)MAX(60,MME_HEADER_MINSIZE+(int)pld_maxlen); } else { *cnf_len = (uint)MAX(60,MME_HEADER_MAXSIZE+(int)pld_maxlen); } memset(mme_tx_buffer,0x00,*cnf_len); #if 1 bitstream_init(&bstr_ctx,mme_tx_buffer,*cnf_len,BITSTREAM_WRITE); bitstream_access(&bstr_ctx,&send_mme->header.un_osa.u64,48); bitstream_access(&bstr_ctx,&send_mme->header.un_oda.u64,48); if (send_mme->header.vlan_tag_flag) bitstream_access(&bstr_ctx,&send_mme->header.un_vlan_tag.u32,32); bitstream_access(&bstr_ctx,&send_mme->header.un_mtype.u32,16); bitstream_access(&bstr_ctx,&send_mme->header.un_mmv.u32,8); bitstream_access(&bstr_ctx,&send_mme->header.un_mmtype.u32,16); bitstream_access(&bstr_ctx,&send_mme->header.un_fmi.u32,16); bitstream_access(&bstr_ctx,&send_mme->result,8); bitstream_access(&bstr_ctx,&send_mme->errcode,8); bitstream_finalise(&bstr_ctx); #endif printf ("%u /***************************************************/\n",cyg_hal_sys_getpid()); printf ("%u /* STA : %d CP : SENDING AN MME (len = %u bytes)...\n", cyg_hal_sys_getpid(), 0/*cp_sta_global.mac_config->tei*/, *cnf_len); #if 1 j = 0; printf("%u /* ",cyg_hal_sys_getpid()); for (i = 0; i < (uint)(*cnf_len / 16); i++) { for (j = 0; j < 16; j++) printf ("%02x.",mme_tx_buffer[(i*16)+j]); printf("\n"); printf("%u /* ",cyg_hal_sys_getpid()); } for (i = 0; i < (*cnf_len % 16); i++) { printf ("%02x.",mme_tx_buffer[(j*16)+i]); } printf("\n"); #endif printf ("%u /***************************************************/\n",cyg_hal_sys_getpid()); }