+ /* For second phase of HS mode */
+ hsscll = hssclh = I2C_INTERNAL_SAMPLING_CLK / (2 * speed);
+
+ hsscll -= I2C_HIGHSPEED_PHASE_TWO_SCLL_TRIM;
+ hssclh -= I2C_HIGHSPEED_PHASE_TWO_SCLH_TRIM;
+ if (((fsscll < 0) || (fssclh < 0)) ||
+ ((fsscll > 255) || (fssclh > 255))) {
+ printf("Error : I2C initializing second phase clock\n");
+ return;
+ }
+
+ scll = (unsigned int)hsscll << 8 | (unsigned int)fsscll;
+ sclh = (unsigned int)hssclh << 8 | (unsigned int)fssclh;
+
+ } else {
+ /* Standard and fast speed */
+ fsscll = fssclh = I2C_INTERNAL_SAMPLING_CLK / (2 * speed);
+
+ fsscll -= I2C_FASTSPEED_SCLL_TRIM;
+ fssclh -= I2C_FASTSPEED_SCLH_TRIM;
+ if (((fsscll < 0) || (fssclh < 0)) ||
+ ((fsscll > 255) || (fssclh > 255))) {
+ printf("Error : I2C initializing clock\n");
+ return;
+ }
+
+ scll = (unsigned int)fsscll;
+ sclh = (unsigned int)fssclh;
+ }
+
+ if (readw (&i2c_base->con) & I2C_CON_EN) {
+ writew (0, &i2c_base->con);