/******************************************************************************/
/*                 Copyright C 1998-2004 Broadcom Corporation                 */
/*                                                                            */
/* This material is the confidential trade secret and proprietary information */
/* of Broadcom Corporation.  It may not be reproduced, used, sold or          */
/* transferred to any third party without the prior written consent of        */
/* Broadcom Corporation.   All rights reserved.                               */
/******************************************************************************/
/*   FileName        : bcm_psdToolbox.c                                       */
/*   Purpose         : Implement the PSD toolbox (see header file)            */
/*   Limitations     : None                                                   */
/*   Author          : Miguel Peeters                                         */
/*   Creation Date   : 2-Feb-2004                                             */
/*   History         : Compiled by CVS                                        */
/******************************************************************************/
/*   $Id: bcm_psdToolbox.c,v 1.5 2004/10/04 15:02:34 hanssens Exp $  */
/******************************************************************************/

#include <string.h>
#include "bcm_psdToolbox.h"

#define BCM_N_MAX 512

/* {{{ local type definitions and static variables */

typedef int16 fix8_8 ;
typedef int32 fix16_16;

/* High precision tssi used to extrapolate the good mask */
typedef struct TssiPointFull TssiPointFull;
struct TssiPointFull {
  uint16  index;
  fix8_8 log_tssi;
};

typedef struct Shape Shape;
struct Shape {
  fix8_8 nomPSD;
  int nBreakpoints;
  TssiPointFull tssiPoint[BCM_TSSI_SHAPE_LEN];
};

static Shape standardShape[BCM_N_SHAPE] =
{
  {
    /* BCM_PSD_CO shape */
    -10240,  /* -40 dBm/Hz */
    4,
    {{0,0},{255,0},{376,-2560},{511,-2944},
     {0,0},{0,0},{0,0},{0,0},
     {0,0},{0,0},{0,0},{0,0},
     {0,0},{0,0},{0,0},{0,0}}
  },
  {
    /* BCM_PSD_FLAT shape */
    -15360,  /* -60 dBm/Hz */
    2,
    {{0,0},{511,0},{0,0},{0,0},
     {0,0},{0,0},{0,0},{0,0},
     {0,0},{0,0},{0,0},{0,0},
     {0,0},{0,0},{0,0},{0,0}}
  },
  {
    /* BCM_PSD_CAB_ANSI shape */
    -12800,  /* -50 dBm/Hz */
    4,
    {{0,-2560},{255,-2560},{376,0},{511,-384},
     {0,0},{0,0},{0,0},{0,0},
     {0,0},{0,0},{0,0},{0,0},
     {0,0},{0,0},{0,0},{0,0}}
  },
  {
    /* BCM_PSD_CAB_ETSI shape */
    -13184,  /* -51.5 dBm/Hz */
    4,
    {{0,-2176},{255,-2176},{323,0},{511,-384},
     {0,0},{0,0},{0,0},{0,0},
     {0,0},{0,0},{0,0},{0,0},
     {0,0},{0,0},{0,0},{0,0}}
  },
  {
    /* BCM_PSD_EXCH_ETSI shape */
    -10240,  /* -40 dBm/Hz */
    4,
    {{0,0},{255,0},{388,-5376},{511,-5376},
     {0,0},{0,0},{0,0},{0,0},
     {0,0},{0,0},{0,0},{0,0},
     {0,0},{0,0},{0,0},{0,0}}
  },
  {
    /* BCM_PSD_EXCH_ANSI shape */
    -10240,  /* -40 dBm/Hz */
    4,
    {{0,0},{255,0},{376,-5120},{511,-5120},
     {0,0},{0,0},{0,0},{0,0},
     {0,0},{0,0},{0,0},{0,0},
     {0,0},{0,0},{0,0},{0,0}}
  }
};

static Shape afeCorrectionShape =
{
  /* shape to compensate AFE attenuation */
  0, 
  4,
  {{0,0},{255,0},{376,640},{511,1024},
   {0,0},{0,0},{0,0},{0,0},
   {0,0},{0,0},{0,0},{0,0},
   {0,0},{0,0},{0,0},{0,0}}
};

/* }}} */

/* {{{ Function to compute the linear interpolation of the tssi */
void interpolation(Shape *shape, fix8_8 *fullTssi)
{
  int k;
  int bp_start, bp_stop;
  int stop_idx;
  fix16_16 slope;
  fix8_8 log_start, log_stop;

  bp_start  = shape->tssiPoint[0].index;
  bp_stop   = shape->tssiPoint[1].index;
  log_start = shape->tssiPoint[0].log_tssi;
  log_stop  = shape->tssiPoint[1].log_tssi;
  stop_idx  = 1;
  slope     = ((log_start-log_stop)<<8)/(bp_start-bp_stop);

  for(k=0;k<BCM_N_MAX;k++)
  {
    if(k>bp_stop)
    {
      stop_idx++;
      bp_start  = bp_stop;
      log_start = log_stop; 
      bp_stop   = shape->tssiPoint[stop_idx].index;
      log_stop  = shape->tssiPoint[stop_idx].log_tssi;
      slope     = ((log_start-log_stop)<<8)/(bp_start-bp_stop);
    }

    fullTssi[k] = ((slope * (k-bp_stop))>>8) + log_stop + shape->nomPSD;
  }
}
/* }}} */

/* {{{ Merge two shapes and return the value in shape1 */
void mergeShape(Shape  *shape1,
                fix8_8 *fullShape1,
                Shape  *shape2,
                fix8_8 *fullShape2)
{
  int bp_idx1, bp_idx2;
  int shape_idx;
  int k;
  Shape mergedShape;
  fix8_8 fullMergedShape[BCM_N_MAX];

  /* Initialize the scan of both shapes */
  bp_idx1 = 0;
  bp_idx2 = 0;
  mergedShape.nBreakpoints = 1;
  mergedShape.nomPSD = 0;
  mergedShape.tssiPoint[0].index = 0;
  if (fullShape1[0]>fullShape2[0])
  {
    shape_idx = 2;
    mergedShape.tssiPoint[0].log_tssi = fullShape2[0];
    fullMergedShape[0]  = fullShape2[0];
  }
  else
  {
    shape_idx = 1;
    mergedShape.tssiPoint[0].log_tssi = fullShape1[0];
    fullMergedShape[0]  = fullShape1[0];
  }

  for(k=1;k<(BCM_N_MAX-1);k++)
  {
    fullMergedShape[k] = (fullShape1[k] < fullShape2[k]) ?
      fullShape1[k] : fullShape2[k];
    if (shape_idx==1)
    {
      /* Check if a new breakpoints must be introduced  */
      /* because the shape2 is below the shape1         */
      if (fullShape2[k]<fullShape1[k])
      {
        mergedShape.tssiPoint[mergedShape.nBreakpoints].index    = k;
        mergedShape.tssiPoint[mergedShape.nBreakpoints].log_tssi = fullShape2[k];
        mergedShape.nBreakpoints++;
        shape_idx = 2;
      }
      /* Check if a new breakpoints must be introduced   */
      /* because the shape1 has a breakpoint             */
      else
        if (k==shape1->tssiPoint[bp_idx1+1].index)
        {
          mergedShape.tssiPoint[mergedShape.nBreakpoints].index = k;
          mergedShape.tssiPoint[mergedShape.nBreakpoints].log_tssi = fullShape1[k];
          mergedShape.nBreakpoints++;
          bp_idx1++;
        }
    }
    else /* shape_idx */
    {
      /* Check if a new breakpoints must be introduced  */
      /* because the shape1 is below the shape2         */
      if (fullShape1[k]<fullShape2[k])
      {
        mergedShape.tssiPoint[mergedShape.nBreakpoints].index    = k;
        mergedShape.tssiPoint[mergedShape.nBreakpoints].log_tssi = fullShape1[k];
        mergedShape.nBreakpoints++;
        shape_idx = 1;
      }
      /* Check if a new breakpoints must be introduced   */
      /* because the shape1 has a breakpoint             */
      else
        if (k==shape2->tssiPoint[bp_idx2+1].index)
        {
          mergedShape.tssiPoint[mergedShape.nBreakpoints].index    = k;
          mergedShape.tssiPoint[mergedShape.nBreakpoints].log_tssi = fullShape2[k];
          mergedShape.nBreakpoints++;
        }
    }
    if (k==shape1->tssiPoint[bp_idx1+1].index)
      bp_idx1++;
    if (k==shape2->tssiPoint[bp_idx2+1].index)
      bp_idx2++;
  }

  /* Add last breakpoint */
  mergedShape.tssiPoint[mergedShape.nBreakpoints].index    = BCM_N_MAX-1;	
  fullMergedShape[BCM_N_MAX-1] = (fullShape1[BCM_N_MAX-1]<fullShape2[BCM_N_MAX-1]) ?
    fullShape1[BCM_N_MAX-1] : fullShape2[BCM_N_MAX-1];
  mergedShape.tssiPoint[mergedShape.nBreakpoints].log_tssi = fullMergedShape[BCM_N_MAX-1];	
  mergedShape.nBreakpoints++;

  *shape1 = mergedShape;
  memcpy(fullShape1, fullMergedShape, BCM_N_MAX*sizeof(fix8_8));
}
/* }}} */

/* {{{ Function to add a RFI band to a shape */
void addRFIband(Shape *shape, fix8_8 *fullTssiShape, int idxStart, int idxStop)
{
  Shape   shapeFeature;
  fix8_8  fullTssiShapeFeature[BCM_N_MAX];
  int    transLow,transHigh;
  int    nBreakpoints;
  fix8_8 psdStart,psdStop;

  psdStart = fullTssiShape[idxStart];
  psdStop  = fullTssiShape[idxStop];
  /* Formula to compute number of transition tones */
  /* (PSD(startTone or stopTone)-10+80)/0.75       */
  transLow  = (((psdStart-10*256+80*256)*4)/3+128)>>8;
  transLow  = (transLow>0) ? idxStart-transLow:idxStart-1;
  transHigh = (((psdStop-10*256+80*256)*4)/3+128)>>8;
  transHigh = (transHigh>0) ? idxStop+transHigh:idxStop+1;

  if((transLow-1)<0)
  {
    /* If out of bond, fixed slope of 0.75dB per tone from the idxStart */
    nBreakpoints = 1;
    shapeFeature.tssiPoint[0].index    = 0;
    shapeFeature.tssiPoint[0].log_tssi = idxStart*192-80*256; /* 192 = 0.75dB */
  }
  else
  {
    nBreakpoints = 3;
    shapeFeature.tssiPoint[0].index    = 0;
    shapeFeature.tssiPoint[0].log_tssi = 0;
    shapeFeature.tssiPoint[1].index    = transLow-1;
    shapeFeature.tssiPoint[1].log_tssi = 0;
    shapeFeature.tssiPoint[2].index    = transLow;
    /* Trick to avoid issue with BP */
    shapeFeature.tssiPoint[2].log_tssi = fullTssiShape[transLow]-1;
  }
  shapeFeature.tssiPoint[nBreakpoints].index      = idxStart;
  shapeFeature.tssiPoint[nBreakpoints++].log_tssi = -80*256;
  shapeFeature.tssiPoint[nBreakpoints].index      = idxStop;
  shapeFeature.tssiPoint[nBreakpoints++].log_tssi = -80*256;
  if((transHigh+1)>(BCM_N_MAX-1))
  {
    /* If out of bond, fixed slope of 0.75dB per tone from the idxStop */
    shapeFeature.tssiPoint[nBreakpoints].index      = BCM_N_MAX-1;
    shapeFeature.tssiPoint[nBreakpoints++].log_tssi = (BCM_N_MAX-1-idxStop)*192-80*256;
  }
  else
  {
    shapeFeature.tssiPoint[nBreakpoints].index      = transHigh;
    /* Trick to avoid BP duplication */
    shapeFeature.tssiPoint[nBreakpoints++].log_tssi = fullTssiShape[transHigh]+1;
    shapeFeature.tssiPoint[nBreakpoints].index      = transHigh+1;
    shapeFeature.tssiPoint[nBreakpoints++].log_tssi = 0;
    shapeFeature.tssiPoint[nBreakpoints].index      = BCM_N_MAX-1;
    shapeFeature.tssiPoint[nBreakpoints++].log_tssi = 0;
  }

  shapeFeature.nBreakpoints = nBreakpoints;
  shapeFeature.nomPSD       = 0;
  interpolation(&shapeFeature, fullTssiShapeFeature);
  mergeShape(shape, fullTssiShape, &shapeFeature, fullTssiShapeFeature);
}
/* }}} */

/* {{{ Function to add a low band transition to a shape */
void addLowBandTransition(Shape *shape,
                          fix8_8 *fullShape,
                          uint8 *adslPlusTxFilterId,
                          uint16 startTone,
                          fix8_8 oobLevel)
{
  Shape  shapeFeature;
  fix8_8 fullTssiShapeFeature[BCM_N_MAX];
  int    transWidth;
  int    psdStartTone;
  int    filterId;

  /* Round the startTone to nearest higher multiple of 10 */
  startTone  = ((startTone-1)/10+1)*10;

  psdStartTone = fullShape[startTone];

  /* oobLevel cannot be higher than psdStartTone */
  if (psdStartTone>oobLevel)
  {
    /* Computation of the transition width */
    transWidth =  (psdStartTone-oobLevel)/563; /* 2.2 dB/tone = 563 */
    if (transWidth==0)
      transWidth = 1;

    shapeFeature.tssiPoint[0].index    = 0;
    shapeFeature.tssiPoint[0].log_tssi = oobLevel + (20<<8);
    shapeFeature.tssiPoint[1].index    = startTone-transWidth;
    shapeFeature.tssiPoint[1].log_tssi = oobLevel + (20<<8);
    shapeFeature.tssiPoint[2].index    = startTone;
    shapeFeature.tssiPoint[2].log_tssi = psdStartTone+1; /* Trick to avoid BP duplication */
    shapeFeature.tssiPoint[3].index    = startTone+1;
    shapeFeature.tssiPoint[3].log_tssi = 0;
    shapeFeature.tssiPoint[4].index    = BCM_N_MAX-1;
    shapeFeature.tssiPoint[4].log_tssi = 0;
    shapeFeature.nomPSD                = 0;
    shapeFeature.nBreakpoints          = 5;
    interpolation(&shapeFeature, fullTssiShapeFeature);
    mergeShape(shape, fullShape, &shapeFeature, fullTssiShapeFeature);
    filterId = (startTone - 130 + 9)/10; /* + 9 to take ceil */
    *adslPlusTxFilterId=max(min(filterId, 15), 1);
  }       
}
/* }}} */

/* {{{ Add two shapes and return the value in shape1 */
void addShape(Shape  *shape1,
              fix8_8 *fullShape1,
              Shape  *shape2,
              fix8_8 *fullShape2)
{
  int bp_idx1, bp_idx2;
  int k;
  Shape newShape;
  fix8_8 fullAddedShape[BCM_N_MAX];

  /* Initialize the scan of both shapes */
  bp_idx1 = 1;
  bp_idx2 = 1;
  newShape.nBreakpoints = 1;
  newShape.nomPSD = 0;
  newShape.tssiPoint[0].index = 0;
  newShape.tssiPoint[0].log_tssi = fullShape1[0] + fullShape2[0];

  for(k=1; k<BCM_N_MAX; k++)
  {
    fullAddedShape[k] = fullShape1[k] + fullShape2[k];

    /* Check if a new breakpoints must be introduced   */
    /* because shape1 or shape2 has a breakpoint       */
    if (k==shape1->tssiPoint[bp_idx1].index ||
        k==shape2->tssiPoint[bp_idx2].index)
    {
      newShape.tssiPoint[newShape.nBreakpoints].index = k;
      newShape.tssiPoint[newShape.nBreakpoints].log_tssi = fullAddedShape[k];
      newShape.nBreakpoints++;

      /* increment input shape bp index when necessary */
      if (k==shape1->tssiPoint[bp_idx1].index)
        bp_idx1++;

      if (k==shape2->tssiPoint[bp_idx2].index)
        bp_idx2++;
    }
  }

  *shape1 = newShape;
  memcpy(fullShape1, fullAddedShape, BCM_N_MAX*sizeof(fix8_8));
}
/* }}} */

/* {{{ Main toolbox function */
void psdToolbox(uint8      psdShape,
                int16      maxPsd,
                uint8      nRFIbands,
                uint16     RFIbands[2*BCM_N_RFI_BANDS],
                uint16     startTone,
                int16      oobLevel,
                TssiShape *customTssiShape,
                int16     *maxTxDeltaPsd,
                uint8     *adslPlusTxFilterId)
{
  Shape   shape;
  Shape   shapeFeature;
  fix8_8  fullTssiShape[BCM_N_MAX];
  fix8_8  fullTssiShapeFeature[BCM_N_MAX];
  int k;
  int maxTssi;

  /* Conversion of psd level in fix8_8 format */
  maxPsd   = (maxPsd<<8)/10;
  oobLevel = (oobLevel<<8)/10;
        
  shape = standardShape[psdShape];

  /* Interpolation of tssi shape */
  interpolation(&shape, fullTssiShape);

  /* Ceiling to max psd */
  if(maxPsd<shape.nomPSD)
  {
    /* Generate shape with ceiling */
    shapeFeature.nBreakpoints          = 2;
    shapeFeature.tssiPoint[0].index    = 0;
    shapeFeature.tssiPoint[0].log_tssi = 0;
    shapeFeature.tssiPoint[1].index    = BCM_N_MAX-1;
    shapeFeature.tssiPoint[1].log_tssi = 0;
    shapeFeature.nomPSD                = maxPsd;
    interpolation(&shapeFeature, fullTssiShapeFeature);
    mergeShape(&shape, fullTssiShape, &shapeFeature, fullTssiShapeFeature);
  }

  /* Add RFI bands if necessary */
  for(k=0; k<min(nRFIbands, BCM_N_RFI_BANDS); k++)
    addRFIband(&shape,fullTssiShape,RFIbands[k*2],RFIbands[k*2+1]);

  /* Add Transition band if necessary */
  if(startTone!=0)
    addLowBandTransition(&shape, fullTssiShape, adslPlusTxFilterId,
                         startTone, oobLevel);
  else
    *adslPlusTxFilterId = 0;

  /* Compensate for Front-End attenuation */
  interpolation(&afeCorrectionShape, fullTssiShapeFeature);
  addShape(&shape, fullTssiShape, &afeCorrectionShape, fullTssiShapeFeature);
  
  /* Scaling to the maximal value */
  maxTssi = -(1<<15);
  for(k=0;k<shape.nBreakpoints;k++)
    if(shape.tssiPoint[k].log_tssi>maxTssi)
      maxTssi = shape.tssiPoint[k].log_tssi;
  for(k=0;k<shape.nBreakpoints;k++)
    shape.tssiPoint[k].log_tssi -= maxTssi;
  shape.nomPSD +=maxTssi;

  /* find tone BCM_N_MAX-1 breakpoint */
  customTssiShape->nrOfBreakPoints=0;
  for(k=0;k<shape.nBreakpoints;k++)
    if (shape.tssiPoint[k].index == BCM_N_MAX-1)
    {
      customTssiShape->nrOfBreakPoints = k+1;
      break;
    }
  
  /* Round values to 0.5 dB and cast them in the correct format */
  for(k=0; k<customTssiShape->nrOfBreakPoints; k++)
    customTssiShape->breakPoint[k] =
      BCM_TSSI_BP(shape.tssiPoint[k].index,
                  (-shape.tssiPoint[k].log_tssi+64)>>7);

  /* currently, psd toolbox only support G.992.5 */
  *maxTxDeltaPsd = ((shape.nomPSD*10)>>8) - PHY_STANDARD_DS_PSD_PLUS;
  customTssiShape->adsl2ProtocolType = 2;
}
/* }}} */

