/*
 * Copyright (C) 2011 CERN (www.cern.ch)
 * Author: Alessandro Rubini
 *
 * Released to the public domain
 */

/*
 * This is the startup thing for hosted environments. It
 * defines main and then calls the main loop.
 */
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <errno.h>
#include <time.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <unistd.h>
#include <fcntl.h>
#include <sys/timex.h>
#include <signal.h>

#include <minipc.h>

#include <ppsi/ppsi.h>
#include <ppsi/set-param.h>
#include <ppsi-wrs.h>
#include <libwr/shmem.h>

#  define WRSW_HAL_RETRIES 1000

#define WRSW_HAL_TIMEOUT 2000000 /* us */

const struct wrh_operations wrh_oper = {
	.locking_enable = wrs_locking_enable,
	.locking_poll = wrs_locking_poll,
	.locking_disable = wrs_locking_disable,
	.locking_reset = wrs_locking_reset,
	.enable_ptracker = wrs_enable_ptracker,

	.adjust_in_progress = wrs_adjust_in_progress,
	.adjust_counters = wrs_adjust_counters,
	.adjust_phase = wrs_adjust_phase,
	.get_clock_period = wrs_get_clock_period,

	.set_timing_mode= wrs_set_timing_mode,
	.get_timing_mode= wrs_get_timing_mode,
	.get_timing_mode_state= wrs_get_timing_mode_state,
};

struct minipc_ch *hal_ch;
struct minipc_ch *ppsi_ch;
struct hal_port_state *hal_ports;
struct hal_shmem_header *hal_shmem;
int hal_nports;
struct wrs_shm_head *ppsi_head;

extern struct pp_ext_hooks  pp_hooks;

static int set_extension(struct pp_instance *ppi, int extension);


static int init_extensions(struct pp_instance *ppi)
{
	struct pp_ext_mem *ppi_ext_data;

	if (!(ppi_ext_data =
			wrs_shm_alloc(ppsi_head,
				      sizeof(struct pp_ext_mem) * (PPSI_EXT_N)))
	   ) {
		return -1;
	}

	/* Clean the allocated memory */
	memset(ppi_ext_data, 0, sizeof(struct pp_ext_mem) * PPSI_EXT_N);

	ppi->ext_mem = ppi_ext_data;

#if CONFIG_HAS_EXT_WR
	/* Add WR extension portDS */
	if (!(ppi_ext_data[PPSI_EXT_WR].ext_dsport =
			wrs_shm_alloc(ppsi_head, sizeof(struct wr_dsport)))
	   ) {
		return -1;
	}

	/* Allocate WR data extension */
	if (!(ppi_ext_data[PPSI_EXT_WR].ext_data =
			wrs_shm_alloc(ppsi_head, sizeof(struct wr_data)))
	   ) {
		return -1;
	}

	/* Set WR extension hooks */
	ppi_ext_data[PPSI_EXT_WR].ext_hooks = &wr_ext_hooks;
#endif

#if CONFIG_HAS_EXT_L1SYNC
	if (!(ppi_ext_data[PPSI_EXT_L1S].ext_dsport =
			wrs_shm_alloc(ppsi_head, sizeof(l1e_ext_portDS_t)))
	   ) {
		return -1;
	}

	/* Allocate L1SYNC data extension */
	if (!(ppi_ext_data[PPSI_EXT_L1S].ext_data =
		wrs_shm_alloc(ppsi_head,sizeof(struct l1e_data)))
	   ) {
		return -1;
	}

	/* Set L1SYNC extension hooks */
	ppi_ext_data[PPSI_EXT_L1S].ext_hooks = &l1e_ext_hooks;
#endif

	return 0;
}


/* Should be called when the extension is changed in runtime */
static int set_extension_runtime_init(struct pp_instance *ppi)
{
	int tmo;

	ppi->pdstate = PP_PDSTATE_NONE;
	/* Default value for PTP. Can be overwritten in specific init */
	ppi->extState =
		(ppi->protocol_extension == PPSI_EXT_NONE)
		? PP_EXSTATE_DISABLE : PP_EXSTATE_ACTIVE;

	/* WRPC 4.2 and possibly old WRS FW does not send delay_req/delay_resp
	 * while locking the PLL. Due to that extension setup can timeout. */
	tmo = is_ext_hook_available(ppi,get_tmo_lstate_detection) ?
			(*ppi->ext_hooks->get_tmo_lstate_detection)(ppi)
			: 20000; // 20s per default
	pp_timeout_set(ppi, PP_TO_PROT_STATE, tmo);

	if (!ppi->link_up) {
		/* Skip extension initialization, it will be done when the link
		 * goes up. */
		return 0;
	}
	if (is_ext_hook_available(ppi, init)
	    && ppi->ext_hooks->init(ppi, NULL, 0)
	    ) {
		pp_diag(ppi, ext, 1, "%s: can't init profile\n",
			__func__);
		return 1;
	}

	if (ppi->state == PPS_SLAVE) {
		/* When the extension/profile is changed the servo is not reset
		 * nor initialized. Initialization of servo (wrh_servo_init) is
		 * done via new_slave */
		if (is_ext_hook_available(ppi, new_slave))
			ppi->ext_hooks->new_slave(ppi, NULL, 0);
		else
			pp_servo_init(ppi);
	}

	return 0;
}

int set_extension_runtime(struct pp_instance *ppi, int extension)
{
	if (set_extension_precheck(ppi, extension) < 0)
		return -1;

	if (set_extension(ppi, extension) < 0) {
		/* Error */
		return -1;
	}

	/* Initialize set extenstion */
	return set_extension_runtime_init(ppi);
}

int set_extension_precheck(struct pp_instance *ppi, int extension)
{
	int profile = ppi->cfg.profile;

	if (extension == PPSI_EXT_NONE)
		return 0;

	if (extension == PPSI_EXT_WR) {
		if (!CONFIG_HAS_EXT_WR)
			return -1;
		if (profile == PPSI_PROFILE_HA_WR
		    || profile == PPSI_PROFILE_CUSTOM) {
			/* Extension is supported for the selected profile */
			return 0;
		}
		return -2;
	}
	if (extension == PPSI_EXT_L1S) {
		if (!CONFIG_HAS_EXT_L1SYNC)
			return -1;
		if (profile == PPSI_PROFILE_HA_WR
		    || profile == PPSI_PROFILE_CUSTOM) {
			/* Extension is supported for the selected profile */
			return 0;
		}
		return -2;
	}

	pp_diag(ppi, config, 1, "BUG: %s: unexpected combination of extension (%d) "
		  "and profile (%d)\n", __func__, extension, profile);
	return -2;
}

static int set_extension(struct pp_instance *ppi, int extension)
{
	pp_diag(ppi, config, 3, "%s: set extension: %s (%d)\n",
		__func__, lut_extension_name[extension], extension);

	switch (extension) {
#if CONFIG_HAS_EXT_WR
	case PPSI_EXT_WR:
		ppi->portDS->ext_dsport = ppi->ext_mem[PPSI_EXT_WR].ext_dsport;
		ppi->ext_data = ppi->ext_mem[PPSI_EXT_WR].ext_data;
		ppi->ext_hooks = ppi->ext_mem[PPSI_EXT_WR].ext_hooks;
		ppi->protocol_extension = PPSI_EXT_WR;

#if CONFIG_HAS_EXT_L1SYNC
		/* Disable L1Sync just in case it was enabled */
		L1E_DSPOR_BS(ppi)->L1SyncState = L1SYNC_DISABLED;
		L1E_DSPOR_BS(ppi)->L1SyncEnabled = FALSE;
#endif /* CONFIG_HAS_EXT_L1SYNC */
		break;
#endif

#if CONFIG_HAS_EXT_L1SYNC
	case PPSI_EXT_L1S:
		ppi->portDS->ext_dsport = ppi->ext_mem[PPSI_EXT_L1S].ext_dsport;
		ppi->ext_data = ppi->ext_mem[PPSI_EXT_L1S].ext_data;
		ppi->ext_hooks = ppi->ext_mem[PPSI_EXT_L1S].ext_hooks;

		ppi->protocol_extension=PPSI_EXT_L1S;

		/* Set L1SYNC state. Must be done here because the init hook is
		 * called only in the initializing state. If the port is not
		 * connected, the initializing is then never called so the
		 * L1SYNC state is invalid (0)
		 */
		L1E_DSPOR_BS(ppi)->L1SyncState = L1SYNC_DISABLED;
		L1E_DSPOR_BS(ppi)->L1SyncEnabled = TRUE;
		break;
#endif

	case PPSI_EXT_NONE:
		ppi->portDS->ext_dsport = NULL;
		ppi->ext_data = NULL;
		ppi->ext_hooks = &pp_hooks;
		ppi->protocol_extension = PPSI_EXT_NONE;

#if CONFIG_HAS_EXT_L1SYNC
		/* Disable L1Sync just in case it was enabled */
		L1E_DSPOR_BS(ppi)->L1SyncState = L1SYNC_DISABLED;
		L1E_DSPOR_BS(ppi)->L1SyncEnabled = FALSE;
#endif /* CONFIG_HAS_EXT_L1SYNC */
		break;

	default:
		fprintf(stderr, "ppsi: Extension not supported\n");
		return -1;
		break;
	}

	set_param_ptpVersion(ppi);

	return 0;
}

static void set_profile_params(struct pp_instance *ppi)
{
	/* When switching a profile, set the default values for the new profile. */

	/* For don't change any parametres when switching to
	 * PPSI_PROFILE_CUSTOM */
	if (ppi->cfg.profile == PPSI_PROFILE_CUSTOM) {
		/* Don't change parameters for the custom profile */
		return;
	}

	if (!is_profile_supported(ppi->cfg.profile)) {
		/* Profile not supported */
		return;
	}

	if (DSPOR(ppi)->logMinDelayReqInterval != lut_profile_logMinDelayReqInterval_default[ppi->cfg.profile]) {
		pp_diag(ppi, config, 1,
			"Seting logMinDelayReqInterval (current %d) to the default value (%d) for %s profile\n",
			DSPOR(ppi)->logMinDelayReqInterval,
			lut_profile_logMinDelayReqInterval_default[ppi->cfg.profile],
			lut_profile_name[ppi->cfg.profile]);
		set_param_inst_logMinDelayReqInterval(ppi, lut_profile_logMinDelayReqInterval_default[ppi->cfg.profile]);
	}

	if (DSPOR(ppi)->logMinPdelayReqInterval != lut_profile_logMinPdelayReqInterval_default[ppi->cfg.profile]) {
		pp_diag(ppi, config, 1,
			"Seting logMinPdelayReqInterval (current %d) to the default value (%d) for %s profile\n",
			DSPOR(ppi)->logMinPdelayReqInterval,
			lut_profile_logMinPdelayReqInterval_default[ppi->cfg.profile],
			lut_profile_name[ppi->cfg.profile]);
		set_param_inst_logMinPdelayReqInterval(ppi, lut_profile_logMinPdelayReqInterval_default[ppi->cfg.profile]);
	}

	if (DSPOR(ppi)->logSyncInterval != lut_profile_logSyncInterval_default[ppi->cfg.profile]) {
		pp_diag(ppi, config, 1,
			"Seting logSyncInterval (current %d) to the default value (%d) for %s profile\n",
			DSPOR(ppi)->logSyncInterval,
			lut_profile_logSyncInterval_default[ppi->cfg.profile],
			lut_profile_name[ppi->cfg.profile]);
		set_param_inst_logSyncInterval(ppi, lut_profile_logSyncInterval_default[ppi->cfg.profile]);
	}
}

static void set_l1sync_params(struct pp_instance *ppi)
{
	/* Force mandatory attributes - Do not take care of the configuration */
	L1E_DSPOR_BS(ppi)->rxCoherentIsRequired = TRUE;
	L1E_DSPOR_BS(ppi)->txCoherentIsRequired = TRUE;
	L1E_DSPOR_BS(ppi)->congruentIsRequired = TRUE;
	L1E_DSPOR_BS(ppi)->optParamsEnabled = FALSE;
}

static int set_profile(struct pp_instance *ppi, int profile, int extension,
		       int startup)
{
	ppi->cfg.profile = profile;
	pp_diag(ppi, config, 3, "%s: set profile: %s (%d)\n",
		__func__, lut_profile_name[ppi->cfg.profile],
		ppi->cfg.profile);

	switch (profile) {
	case PPSI_PROFILE_PTP :
		/* Disable extension */
		if (set_extension(ppi, PPSI_EXT_NONE) < 0) {
			/* Error */
			return -1;
		}

		/* Disable extension autonegotiation in case it was enabled */
		ppi->cfg.extAutonegEnable = 0;

		/* Do not take care of L1SYNC */
		enable_asymmetryCorrection(ppi, ppi->cfg.asymmetryCorrectionEnable);

		if (!startup) {
			set_profile_params(ppi);
		}

		break;

	case PPSI_PROFILE_HA_WR:
#if CONFIG_HAS_PROFILE_HA_WR
		if (startup) {
			/* Change the extension to the configured one at
			 * startup */
			if (set_extension(ppi, extension) < 0) {
				/* Error */
				return -1;
			}
		} else if (CONFIG_HAS_EXT_WR
			   /* if autonegotiation is used use L1Sync as
			    * extension */
			   && ppi->cfg.extAutonegEnable == FALSE) {
			if (set_extension(ppi, PPSI_EXT_WR) < 0) {
				/* Error */
				return -1;
			}
		} else if (CONFIG_HAS_EXT_L1SYNC) {
			if (set_extension(ppi, PPSI_EXT_L1S) < 0) {
				/* Error */
				return -1;
			}
		} else {
			if (set_extension(ppi, PPSI_EXT_NONE) < 0) {
				/* Error */
				return -1;
			}
		}

		if (!startup) {
			set_profile_params(ppi);
		}

		if (CONFIG_HAS_EXT_L1SYNC) {
			/* Set L1Sync params even for WR profile, since we allow
			 * configuration WR profile with L1Sync extension */
			set_l1sync_params(ppi);
		}

		enable_asymmetryCorrection(ppi, TRUE);
		break;
#else
		fprintf(stderr, "ppsi: Profile HA/WR not supported\n");
		return -1;
#endif /* CONFIG_HAS_PROFILE_HA_WR */

	case PPSI_PROFILE_CUSTOM :
#if CONFIG_HAS_PROFILE_CUSTOM
		if (startup
		    && set_extension(ppi, extension) < 0) {
			/* Error */
			return -1;
		}
#if  CONFIG_HAS_EXT_L1SYNC
		if (!startup) {
			/* Set extension and other parameters used by CUSTOM
			 * profile only at startup. The intention is that
			 * no parameters are changed at the switch to CUSTOM
			 * profile in runtime. If some parametres need
			 * to be adjusted when running CUSTOM profile, they
			 * shall be changed using other ways (e.g. runtime
			 * reconfiguration). */
			break;
		}
		if (ppi->cfg.l1SyncEnabled ) {
			/* Read L1SYNC parameters from configuration */
			L1E_DSPOR_BS(ppi)->rxCoherentIsRequired = ppi->cfg.l1SyncRxCoherentIsRequired;
			L1E_DSPOR_BS(ppi)->txCoherentIsRequired = ppi->cfg.l1SyncTxCoherentIsRequired;
			L1E_DSPOR_BS(ppi)->congruentIsRequired = ppi->cfg.l1SyncCongruentIsRequired;
			L1E_DSPOR_BS(ppi)->optParamsEnabled = ppi->cfg.l1SyncOptParamsEnabled;
			if (L1E_DSPOR_BS(ppi)->optParamsEnabled) {
				L1E_DSPOR_OP(ppi)->timestampsCorrectedTx = ppi->cfg.l1SyncOptParamsTimestampsCorrectedTx;
			}
		}
#endif /* CONFIG_HAS_EXT_L1SYNC*/
		enable_asymmetryCorrection(ppi, ppi->cfg.asymmetryCorrectionEnable);
#else
		fprintf(stderr, "ppsi: Profile CUSTOM not supported\n");
		return -1;
#endif
		break;

	default:
		fprintf(stderr, "ppsi: Not supported profile %d\n", profile);
		return -1;
	}

	return 0;
}

int set_profile_runtime(struct pp_instance *ppi, int profile)
{
	if (set_profile(ppi, profile, 0 /* ignored */, 0 /* no startup */))
		return 1;

	/* Initialize set extenstion */
	return set_extension_runtime_init(ppi);
}

/**
 * Enable/disable asymmetry correction
 */
void enable_asymmetryCorrection(struct pp_instance *ppi, Boolean enable ) {
	if ( (ppi->asymmetryCorrectionPortDS.enable=enable)==TRUE ) {
		/* Enabled: The delay asymmetry will be calculated */

		ppi->asymmetryCorrectionPortDS.scaledDelayCoefficient =( ppi->cfg.scaledDelayCoefficient != 0) ?
			ppi->cfg.scaledDelayCoefficient :
			(RelativeDifference)(ppi->cfg.delayCoefficient * REL_DIFF_TWO_POW_FRACBITS);
		ppi->portDS->delayAsymCoeff=pp_servo_calculateDelayAsymCoefficient(ppi->asymmetryCorrectionPortDS.scaledDelayCoefficient);
	}
	ppi->asymmetryCorrectionPortDS.constantAsymmetry=picos_to_interval(ppi->cfg.constantAsymmetry_ps);
}

/* Functions to set global parameters */

void set_param_global_prio1(struct pp_globals *ppg, int prio1)
{
	ppg->rt_opts->priority1 = prio1;
	bmc_apply_configured_device_attributes(ppg);
}

void set_param_global_prio2(struct pp_globals *ppg, int prio2)
{
	ppg->rt_opts->priority2 = prio2;
	bmc_apply_configured_device_attributes(ppg);
}

void set_param_global_bmca(struct pp_globals *ppg, int bmca)
{
	ppg->rt_opts->bmca = bmca;
	pp_set_bmca(ppg, bmca);
}

/* Functions to set parameters specific to instance */

void set_param_inst_logMinDelayReqInterval(struct pp_instance *ppi,
					   int logMinDelayReqInterval)
{
	int millisec;

	DSPOR(ppi)->logMinDelayReqInterval = logMinDelayReqInterval;
	ppi->cfg.min_delay_req_interval = logMinDelayReqInterval;
	millisec = pp_timeout_log_to_ms(logMinDelayReqInterval);

	if (is_delayMechanismE2E(ppi))
		pp_timeout_set_rename(ppi, PP_TO_REQUEST, millisec);
}

void set_param_inst_logMinPdelayReqInterval(struct pp_instance *ppi,
					    int logMinPdelayReqInterval)
{
	int millisec;

	DSPOR(ppi)->logMinPdelayReqInterval = logMinPdelayReqInterval;
	ppi->cfg.min_pdelay_req_interval = logMinPdelayReqInterval;

	millisec = pp_timeout_log_to_ms(logMinPdelayReqInterval);

	if (is_delayMechanismP2P(ppi))
		pp_timeout_set_rename(ppi, PP_TO_REQUEST, millisec);
}

void set_param_inst_logSyncInterval(struct pp_instance *ppi,
				    int logSyncInterval)
{
	int millisec;

	DSPOR(ppi)->logSyncInterval = logSyncInterval;
	ppi->cfg.sync_interval = logSyncInterval;

	millisec = pp_timeout_log_to_ms(logSyncInterval);
	pp_timeout_set_rename(ppi, PP_TO_SYNC_SEND, millisec);
}

static char *strCodeOpt="CODEOPT=("
#if CONFIG_HAS_CODEOPT_EPC_ENABLED
		" EPC"
#endif
#if CONFIG_HAS_CODEOPT_SO_ENABLED
		" SO"
#endif
#if CONFIG_HAS_CODEOPT_SINGLE_FMASTER
		" SFM"
#endif
#if CONFIG_HAS_CODEOPT_SINGLE_PORT
		" SP"
#endif
		")";

int main(int argc, char **argv)
{
	struct pp_globals *ppg;
	unsigned long seed;
	struct timex t;
	int i, hal_retries;
	struct wrs_shm_head *hal_head;

	setbuf(stdout, NULL);

	pp_printf("PPSi. Commit %s, built on " __DATE__ ", %s\n",
		PPSI_VERSION,
		strCodeOpt);

	/* Check for -h or --help in params list */
	pp_parse_cmdline_early(argc, argv);

	/* check if there is another instance of PPSi already running */
	ppsi_head = wrs_shm_get(wrs_shm_ptp, "", WRS_SHM_READ);
	if (!ppsi_head) {
		pp_printf("Unable to open shm for PPSi! Unable to check if "
			  "there is another PPSi instance running. Error: %s\n",
			  strerror(errno));
		exit(1);
	}

	/* check if pid is 0 (shm not filled) or process with provided
	 * pid does not exist (probably crashed) */
	{
		int nbTry=1;

		while ( nbTry >= 0 ) {
			if ((ppsi_head->pid != 0) && (kill(ppsi_head->pid, 0) == 0)) {
				nbTry--;
				sleep(1);
			}
			else
				break;
		}
		if ( nbTry<0 ) {
			wrs_shm_put(ppsi_head);
			pp_printf("Fatal: There is another PPSi instance running. "
				  "Exit...\n\n");
			exit(1);
		}
	}

	/* it will be opened later for write */
	wrs_shm_put(ppsi_head);

	/* try connecting to HAL multiple times in case it's still not ready */
	hal_retries = WRSW_HAL_RETRIES;
	while (hal_retries) { /* may be never, if built without WR extension */
		hal_ch = minipc_client_create(WRSW_HAL_SERVER_ADDR,
					      MINIPC_FLAG_VERBOSE);
		if (hal_ch)
			break;
		hal_retries--;
		usleep(WRSW_HAL_TIMEOUT);
	}

	if (!hal_ch) {
		pp_printf("ppsi: could not connect to HAL RPC");
		exit(1);
	}

	/* If we connected, we also know "for sure" shmem is there */
	hal_head = wrs_shm_get(wrs_shm_hal,"", WRS_SHM_READ);
	if (!hal_head || !hal_head->data_off) {
		pp_printf("ppsi: Can't connect with HAL "
			  "shared memory\n");
		exit(1);
	}
	if (hal_head->version != HAL_SHMEM_VERSION) {
		pp_printf("ppsi: unknown HAL's shm version %i "
			  "(known is %i)\n", hal_head->version,
			  HAL_SHMEM_VERSION);
		exit(1);
	}

	hal_shmem = (void *)hal_head + hal_head->data_off;
	hal_nports = hal_shmem->nports;

	hal_ports = wrs_shm_follow(hal_head, hal_shmem->ports);

	if (!hal_ports) {
		pp_printf("ppsi: unable to follow hal_ports pointer "
			  "in HAL's shmem\n");
		exit(1);
	}

	/* And create your own channel, until we move to shmem too */
	ppsi_ch = minipc_server_create("ptpd", 0);
	if (!ppsi_ch) { /* FIXME should we retry ? */
		pp_printf("ppsi: could not create minipc server\n");
		exit(1);
	}

	ppsi_head = wrs_shm_get(wrs_shm_ptp, "ppsi",
				WRS_SHM_WRITE | WRS_SHM_LOCKED);
	if (!ppsi_head) {
		fprintf(stderr, "Fatal: could not create shmem: %s\n",
			strerror(errno));
		exit(1);
	}
	ppsi_head->version = WRS_PPSI_SHMEM_VERSION;

	ppg = wrs_shm_alloc(ppsi_head, sizeof(*ppg));
	ppg->defaultDS = wrs_shm_alloc(ppsi_head, sizeof(*ppg->defaultDS));
	ppg->currentDS = wrs_shm_alloc(ppsi_head, sizeof(*ppg->currentDS));
	ppg->parentDS =  wrs_shm_alloc(ppsi_head, sizeof(*ppg->parentDS));
	ppg->timePropertiesDS = wrs_shm_alloc(ppsi_head,sizeof(*ppg->timePropertiesDS));
	ppg->gmPrev = wrs_shm_alloc(ppsi_head, sizeof(*ppg->gmPrev));
	ppg->rt_opts = &__pp_default_rt_opts;

	ppg->max_links = PP_MAX_LINKS;

	ppg->arch_glbl_data = wrs_shm_alloc(ppsi_head, sizeof(wrs_arch_data_t));
	ppg->pp_instances = wrs_shm_alloc(ppsi_head,
				     ppg->max_links * sizeof(struct pp_instance));

	if ((!ppg->arch_glbl_data) || (!ppg->pp_instances)) {
		fprintf(stderr, "ppsi: out of memory\n");
		exit(1);
	}
	/* Set default configuration value for all instances */
	for (i = 0; i < ppg->max_links; i++) {
		memcpy(&INST(ppg, i)->cfg, &__pp_default_instance_cfg,sizeof(__pp_default_instance_cfg));
	}

	/* Set offset here, so config parsing can override it */
	memset(&t, 0, sizeof(t));
	if (adjtimex(&t) >= 0) {
		ppg->timePropertiesDS->currentUtcOffset = (Integer16)t.tai;
	}

	if (pp_parse_cmdline(ppg, argc, argv) != 0)
		return -1;

	/* If no item has been parsed, provide a default file or string */
	if (ppg->cfg.cfg_items == 0)
		pp_config_file(ppg, 0, PP_DEFAULT_CONFIGFILE);
	if (ppg->cfg.cfg_items == 0) {
		/* Default configuration for switch is all ports - Priority given to HA */
		char s[128];

		for (i = 0; i < WRS_NUMBER_PHYSICAL_PORTS; i++) {
			sprintf(s, "port %i; iface wri%i; proto raw;", i + 1, i + 1);
			if (CONFIG_HAS_PROFILE_HA_WR) {
				sprintf(s, "profile ha_wr;");
				if (CONFIG_HAS_EXT_L1SYNC)
					sprintf(s, "extension ha;");
				else if  (CONFIG_HAS_EXT_WR)
					sprintf(s, "extension wr");
			} else
				sprintf(s, "profile ptp;");
			pp_config_string(ppg, s);
		}
	}
	for (i = 0; i < ppg->nlinks; i++) {
		struct pp_instance *ppi= INST(ppg, i);

		ppi->ch[PP_NP_EVT].fd = -1;
		ppi->ch[PP_NP_GEN].fd = -1;

		ppi->glbs = ppg;
		ppi->vlans_array_len = CONFIG_VLAN_ARRAY_SIZE;
		ppi->port_idx = i; /* port_idx is also filled in pp_init_globals
				    * in proto-standard/open-close.c, but make
				    * it available also earlier. */
		ppi->iface_name = ppi->cfg.iface_name;
		ppi->port_name = ppi->cfg.port_name;
		ppi->delayMechanism = ppi->cfg.delayMechanism;
		ppi->portDS = wrs_shm_alloc(ppsi_head, sizeof(*ppi->portDS));
		ppi->servo = wrs_shm_alloc(ppsi_head, sizeof(*ppi->servo));
		ppi->ext_hooks=&pp_hooks; /* Default value. Can be overwritten by an extension */
		ppi->ptp_support=TRUE;
		if (ppi->portDS) {
			if (init_extensions(ppi) < 0)
				goto exit_out_of_memory;

			ppi->protocol_extension = ppi->cfg.extension;
			if (set_profile(ppi, ppi->cfg.profile, ppi->cfg.extension, 1 /* startup */) < 0) {
				fprintf(stderr,
					"ppsi: Profile %d not supported\n",
					ppi->cfg.profile);
				exit(1);
			}

			/* Parameters profile independent */
			ppi->timestampCorrectionPortDS.egressLatency=picos_to_interval(ppi->cfg.egressLatency_ps);
			ppi->timestampCorrectionPortDS.ingressLatency=picos_to_interval(ppi->cfg.ingressLatency_ps);
			ppi->timestampCorrectionPortDS.messageTimestampPointLatency=0;
			ppi->portDS->masterOnly= ppi->cfg.masterOnly; /* can be overridden in pp_init_globals() */
		} else {
			goto exit_out_of_memory;
		}

		/* The following default names depend on TIME= at build time */
		ppi->n_ops = &DEFAULT_NET_OPS;
		ppi->t_ops = &DEFAULT_TIME_OPS;

		ppi->__tx_buffer = malloc(PP_MAX_FRAME_LENGTH);
		ppi->__rx_buffer = malloc(PP_MAX_FRAME_LENGTH);

		if (!ppi->__tx_buffer || !ppi->__rx_buffer) {
			goto exit_out_of_memory;
		}
	}

	pp_init_globals(ppg, &__pp_default_rt_opts);

	{
		int nbRetry;
		int enablePPS;
		wrh_timing_mode_pll_state_t timing_mode_pll_state;
		struct pp_instance *ppi=INST(ppg,0);
		wrh_timing_mode_t current_timing_mode;
		int ret=WRH_OPER()->get_timing_mode(ppg,&current_timing_mode);

		if (ret<0) {
			fprintf(stderr, "ppsi: Cannot get current timing mode\n");
			exit(1);
		}

		WRS_ARCH_G(ppg)->timingModeLockingState=WRH_TM_LOCKING_STATE_LOCKING;

		switch (current_timing_mode) {
		case WRH_TM_FREE_MASTER :
			// FR mode is OK for starting in all cases. We are not going to touch it
			// -> GM will be set in the main loop
			// -> BC will be set when a port will become slave
			WRS_ARCH_G(ppg)->timingMode=WRH_TM_FREE_MASTER; // set here because set_timing_mode() is not called
			break;
		case WRH_TM_GRAND_MASTER :
			if ( ppg->defaultDS->clockQuality.clockClass == PP_PTP_CLASS_GM_LOCKED  ) {
				// Already the correct timing mode. Not touched to avoid an unlock of the PLL
				WRS_ARCH_G(ppg)->timingMode=WRH_TM_GRAND_MASTER; // set here because set_timing_mode() is not called
				break;
			}
			// No break here to set the timing mode to FR
		case WRH_TM_BOUNDARY_CLOCK :
		case WRH_TM_DISABLED :
			// Must be reseted to FR
			WRH_OPER()->set_timing_mode(ppg,WRH_TM_FREE_MASTER);
			break;
		}

		/* Waiting for PLL locking. We do not need a precise time-out here */
		/* We are waiting up to 3s for PLL locking.
		 * We do that to avoid to jump too quickly to a degraded clock class.
		 */
		nbRetry=2;
		while(nbRetry>0) {
			int ret=WRH_OPER()->get_timing_mode_state(ppg,&timing_mode_pll_state);
			if ( ret==0 && timing_mode_pll_state==WRH_TM_PLL_STATE_LOCKED )
				break;
			sleep(1); // wait 1s
			nbRetry--;
		}
		if ( timing_mode_pll_state==WRH_TM_PLL_STATE_UNLOCKED) {
				WRS_ARCH_G(ppg)->timingModeLockingState = WRH_TM_LOCKING_STATE_LOCKING;
		} else if (timing_mode_pll_state==WRH_TM_PLL_STATE_LOCKED) {
			TOPS(ppi)->set(ppi,NULL); // GM locked: set the time
			WRS_ARCH_G(ppg)->timingModeLockingState = WRH_TM_LOCKING_STATE_LOCKED;
		}

		/* Enable the PPS generation only if
		 * - Grand master and PLL is locked
		 * OR
		 * - Free running master (no condition required)
		 * OR
		 * - Timing output is forced (for testing only)
		 */
		enablePPS=(WRS_ARCH_G(ppg)->timingModeLockingState== WRH_TM_LOCKING_STATE_LOCKED &&
				ppg->defaultDS->clockQuality.clockClass == PP_PTP_CLASS_GM_LOCKED) ||
				GOPTS(ppg)->forcePpsGen;
		TOPS(ppi)->enable_timing_output(ppg,enablePPS);
	}

	seed = (unsigned long) time(NULL);
	if (getenv("PPSI_DROP_SEED"))
		seed = (unsigned long) atoi(getenv("PPSI_DROP_SEED"));
	ppsi_drop_init(ppg, seed);

	wrs_init_ipcserver(ppg, ppsi_ch);

	/* release lock from wrs_shm_get */
	wrs_shm_write(ppsi_head, WRS_SHM_WRITE_END);

	wrs_main_loop(ppg);
	return 0; /* never reached */

	exit_out_of_memory:;
	fprintf(stderr, "ppsi: out of memory\n");
	exit(1);
}