kernel_interface.c

用于TM1300/PNX1300系列DSP（主要用于视频处理）的操作系统pSOS。包括全部源码

#include "kernel_interface.h"
#include "const.h"

#ifdef __TCS__
#include "tm1/tmInterrupts.h"
#else
#include "tmInterrupts.h"
#endif

/***********************************************/
/* External Declarations                       */
/***********************************************/


extern struct SD_parms SysVars;

ULONG My_Node;         

#define  FIELD_OFFSET(type, field)  ((int)(&((type *)0)->field))

static ULONG pcsw;
static void (*Announce_pkt)();

/************************************************************************/
/* get_masternum:  Obtain the Master Node Number in the header          */
/* input:       none 							*/
/* output:      none 							*/
/* return:      master node number 					*/
/************************************************************************/

USHORT get_masternum(void)
{
	return MASTER;
}

/***********************************************/
/* _k_terminate: remove node from system       */
/*  INPUTS: node to be removed                 */
/*          error code                         */
/*          flags                              */
/* RETURNS:                                    */
/***********************************************/
ULONG _k_terminate(ULONG node, ULONG fcode, ULONG flags)
{
  if (node > SysVars.Kism)
        return (ERR_NODENO);

  if (node == MASTER)
        return (ERR_MASTER);

  if (chk_roster(node)) {
	Dir->queue_addr[node] = 0;
        tnt_insert(fcode, node);
  }
  return 0;
}

/***********************************************/
/* KiNotify: Notify psos (sched) of packet     */
/***********************************************/

extern short *pkd_bitfld;


void KiNotify() 
{
  *pkd_bitfld |= PKT_MASK;
}

void KiNotify_Handler() 
{
# pragma TCS_handler
  ienter();
  *pkd_bitfld |= PKT_MASK;
  ireturn();
}

/***********************************************/
/* lock: set queue lock and disable interrupts */
/*                                             */
/*  INPUTS: lock to set                        */
/* RETURNS: non-zero error, else success       */
/***********************************************/
static ULONG lock(ULONG lock_ptr[])
{
  volatile ULONG *vlock_ptr;
  ULONG spin;
  ULONG i;
  unsigned char sum;

  vlock_ptr= (volatile ULONG*)lock_ptr;

  for(;;) {

    pcsw = intClearIEN();

    vlock_ptr[My_Node] = 1;
    sum = 0;
    for(i = 1; i<=SysVars.Kism; i++)
      sum += vlock_ptr[i];

    if(sum==1) return 0;

    vlock_ptr[My_Node] = 0;

    intRestoreIEN(pcsw);

    /* wait */
    for(spin = 0; spin < (SPIN_VAL * My_Node * 2); spin++) ;
  }
}
/***********************************************/
/* unlock: clear queue lock and                */
/*               restore interrupts            */
/*                                             */
/*  INPUTS: lock to clear                      */
/* RETURNS:                                    */
/***********************************************/
static void unlock(ULONG lock_ptr[])
{
  lock_ptr[My_Node] = 0;
  intRestoreIEN(pcsw);
}

/***********************************************/
/* init_queue: initialize my receive queue     */
/***********************************************/
static ULONG init_queue()
{
  ULONG i;

  My_Queue->msg_head = 0xFFFFFFFF;
  My_Queue->msg_tail = 0xFFFFFFFF;
  My_Queue->free_head = 0;

  for(i = 0; i<NENVELOPES; i++) {
    My_Queue->envelope[i].source=My_Node;
    My_Queue->envelope[i].index=i;
    My_Queue->envelope[i].next=i+1;
  }
 
  My_Queue->envelope[NENVELOPES-1].next = 0xFFFFFFFF;

  return 0;
}

/***********************************************/
/* ki_init: initialize kernel interface        */
/***********************************************/
static ULONG ki_init(void (*ap_addr)())
{
  ULONG node, i;
  int shmid;
  intInstanceSetup_t setup;
  volatile DIRECTORY *vDir;

  My_Node = SysVars.SmNode;

  if(intOpen(KI_INT)) {
    return FAT_SIG;
  }
  setup.enabled = True;
  setup.handler = KiNotify_Handler;
  setup.priority = intPRIO_6;
  setup.level_triggered = False;

  if(intInstanceSetup(KI_INT,&setup)) {
    return FAT_SIG;
  }

  /* check for max number of nodes */
  if((SysVars.Kism > SMEM_MAXNODES) ||
     (SysVars.SmNode > SysVars.Kism) ||
     (SysVars.SmNode == 0))
    return FAT_NOD;

  Announce_pkt = ap_addr;
  
  if(init_shmem()) return FAT_SHM;
  if(init_queue()) return FAT_QUEUE;

  vDir= (volatile DIRECTORY *)Dir;

  vDir->queue_addr[My_Node]= My_Queue;

  /* check if MASTER is running */
  while(vDir->queue_addr[MASTER] == 0) {}

  return 0;
}

/***********************************************/
/* ki_getpkb: allocate a packet buffer         */
/*                                             */
/*  INPUTS: node_num = destination node        */
/*          pkbPtrPtr = ptr to packet          */
/*                      buffer ptr             */
/* RETURNS: non-zero error, else success       */
/* OUTPUTS: pointer to allocated buffer        */
/*          through pkbPtrPtr                  */
/***********************************************/
static ULONG ki_getpkb(ULONG node_num, ULONG **pkbPtrPtr)
{
  ULONG index, err;

  if (Dir->queue_addr[node_num] == 0) {
    _k_terminate(node_num, FAT_NOQ, 0);
    return FAT_NOQ;
  }
  if(err = lock(Dir->queue_addr[node_num]->fq_lock))
    return err;

  if(Dir->queue_addr[node_num]->free_head == 0xFFFFFFFF) {
    unlock(Dir->queue_addr[node_num]->fq_lock);
    return FAT_NOPB;
  }

  index = Dir->queue_addr[node_num]->free_head;
  Dir->queue_addr[node_num]->free_head = Dir->queue_addr[node_num]->envelope[index].next;

  unlock(Dir->queue_addr[node_num]->fq_lock);

  *pkbPtrPtr = Dir->queue_addr[node_num]->envelope[index].packet;

  return 0;
}

/***********************************************/
/* ki_retpkb: return a packet buffer           */
/*                                             */
/*  INPUTS: pkb_ptr = ptr to packet            */
/* RETURNS: non-zero error, else success       */
/***********************************************/
static ULONG ki_retpkb(ULONG *pkb_ptr)
{
  ENVELOPE *env_ptr;
  ULONG err;

  env_ptr = (ENVELOPE *)((ULONG)pkb_ptr - OFFSET(ENVELOPE, packet));

  if(err = lock(My_Queue->fq_lock))
    return err;

  env_ptr->next = My_Queue->free_head;
  My_Queue->free_head = env_ptr->index;

  unlock(My_Queue->fq_lock);

  return 0;
}

/***********************************************/
/* ki_send: copy a packet to another node      */
/*          return the packet to the free list */
/*                                             */
/*  INPUTS: pkb_size                           */
/*          destnode                           */
/*          pkb_ptr                            */
/* RETURNS: non-zero error, else success       */
/***********************************************/
static ULONG ki_send(ULONG pkb_size, ULONG destnode, ULONG *pkb_ptr)
{
  ULONG err;
  ENVELOPE *env_ptr;
  ULONG *dst_pkb_ptr;

  env_ptr = (ENVELOPE *)((ULONG)pkb_ptr - FIELD_OFFSET(ENVELOPE, packet));

  if(env_ptr->source != destnode) {
     if(err = ki_getpkb(destnode, &dst_pkb_ptr)) {
       ki_retpkb(pkb_ptr);
       return err;
     }
     ki_bcopy(pkb_ptr, dst_pkb_ptr, pkb_size);
     ki_retpkb(pkb_ptr);
     pkb_ptr= dst_pkb_ptr;
     env_ptr = (ENVELOPE *)((ULONG)pkb_ptr - FIELD_OFFSET(ENVELOPE, packet));
  }

  if(err = lock(Dir->queue_addr[destnode]->mq_lock))
    return err;

  env_ptr->next = 0xFFFFFFFF;
  
  if(Dir->queue_addr[destnode]->msg_head == 0xFFFFFFFF) {
    Dir->queue_addr[destnode]->msg_head = Dir->queue_addr[destnode]->msg_tail = env_ptr->index;
  }
  else {
    Dir->queue_addr[destnode]->envelope[Dir->queue_addr[destnode]->msg_tail].next = env_ptr->index;
    Dir->queue_addr[destnode]->msg_tail = env_ptr->index;
  }

  intRaise_M(destnode-1,KI_INT);

  unlock(Dir->queue_addr[destnode]->mq_lock);

  return 0;
}

/***********************************************/
/*  ki_receive: remove a pkt from the msg queue*/
/*              pass it to pSOS+               */
/*                                             */
/*      INPUTS: pkbPtrPtr = ptr to packet      */
/*                          buffer ptr         */
/*     OUTPUTS: packet address                 */
/*              NULL if no packet waiting      */
/* RETURNS: non-zero error, else success       */
/***********************************************/
static ULONG ki_receive(ULONG **pkbPtrPtr)
{
  ENVELOPE *env_ptr;
  ULONG index;
  ULONG err, i;

  if(err = lock(My_Queue->mq_lock))
    return err;
  
  index = My_Queue->msg_head;

  if(index == 0xFFFFFFFF) {
    *pkbPtrPtr = (ULONG *)NULL;
  }
  else {
    env_ptr = &(My_Queue->envelope[index]);
    My_Queue->msg_head = env_ptr->next;

    if(My_Queue->msg_head == 0xFFFFFFFF)
      My_Queue->msg_tail = 0xFFFFFFFF;

    *pkbPtrPtr = My_Queue->envelope[index].packet;
  }

  unlock(My_Queue->mq_lock);

  return 0;
}

/***********************************************/
/* ki_check: poll for incoming packets         */
/*           If a packet is waiting on the     */
/*           queue, call KiNotify              */
/***********************************************/
ULONG ki_check()
{
  if(My_Queue->msg_head != ((ULONG)0xFFFFFFFF)) {
    (*Announce_pkt)();
    return 0;
  }
  else return 1;
}

/***********************************************/
/* ki_call                                     */
/***********************************************/
ULONG ki_call(ULONG fcode, ULONG parm1, ULONG parm2, ULONG parm3, ULONG parm4, ULONG parm5)
{
  ULONG err;

  if(fcode==KI_TIME || fcode==KI_ROSTER)
    return 0;

  switch(fcode) {
  case KI_INIT:
    err = ki_init((void (*)())parm1);
    break;
  case KI_GETPKB:
    err = ki_getpkb((ULONG)parm1,(ULONG **)parm2);
    break;
  case KI_RETPKB:
    err = ki_retpkb((ULONG *)parm1);
    break;
  case KI_SNDPKB:
    err = ki_send((ULONG)parm1,(ULONG)parm2,(ULONG *)parm3);
    break;
  case KI_RECEIVE:
    err = ki_receive((ULONG **)parm1);
    break;
  case KI_CHECK:
    err = ki_check();
    break;
  case KI_BRDPKB:
  default:
    err = FAT_NOTSUPP;
    break;
  }
  return err;
}